//#define NOMINMAX
//#pragma warning(disable:4996)

#include <algorithm>
#include <limits>
#include <thread>
#include <mutex>

#include <SDL2/SDL_timer.h>

#include "BSP.h"
#include "GLRenderer.h"
#include "Camera.h"
#include "RayTracing.h"
#include "GLMesh.h"
//#include "globals.h"
#include "leak.h"

using namespace std;


float color_table[1024][3];


Brush::Brush(vector<float>& sides) {

	sides_.swap(sides);

	planes_.resize(sides_.size()/9);

	bbox_min_[0] = bbox_min_[1] = bbox_min_[2] = std::numeric_limits<float>::max();
	bbox_max_[0] = bbox_max_[1] = bbox_max_[2] = -std::numeric_limits<float>::max();

	float ccenter[3] = { 0.0f, 0.0f, 0.0f };

	for(unsigned int i = 0; i < sides_.size(); i += 9)
	{
		float v1[3] = {sides_[i + 3] - sides_[i], sides_[i + 4] - sides_[i + 1], sides_[i + 5] - sides_[i + 2]};
		float v2[3] = {sides_[i + 6] - sides_[i], sides_[i + 7] - sides_[i + 1], sides_[i + 8] - sides_[i + 2]};
		Cross(planes_[i/9].n, v1, v2);
		Normalize(planes_[i/9].n);
		//cout << "normal:  " << planes_[i/9].n[0] << " " << planes_[i/9].n[1] << " " << planes_[i/9].n[2] << endl;
		float p[3] = {sides_[i], sides_[i + 1], sides_[i + 2]};
		planes_[i/9].d = -dot(planes_[i/9].n, p);
		//cout << "d: " << planes_[i/9].d << endl;

		bbox_min_[0] = min(bbox_min_[0], min(sides_[i], min(sides_[i + 3], sides_[i + 6])));
		bbox_min_[1] = min(bbox_min_[1], min(sides_[i + 1], min(sides_[i + 4], sides_[i + 7])));
		bbox_min_[2] = min(bbox_min_[2], min(sides_[i + 2], min(sides_[i + 5], sides_[i + 8])));

		bbox_max_[0] = max(bbox_max_[0], max(sides_[i], max(sides_[i + 3], sides_[i + 6])));
		bbox_max_[1] = max(bbox_max_[1], max(sides_[i + 1], max(sides_[i + 4], sides_[i + 7])));
		bbox_max_[2] = max(bbox_max_[2], max(sides_[i + 2], max(sides_[i + 5], sides_[i + 8])));

		ccenter[0] += sides_[i] + sides_[i + 3] + sides_[i + 6];
		ccenter[1] += sides_[i + 1] + sides_[i + 4] + sides_[i + 7];
		ccenter[2] += sides_[i + 2] + sides_[i + 5] + sides_[i + 8];

	}

	center_[0] = (bbox_min_[0] + bbox_max_[0]) * 0.5f;
	center_[1] = (bbox_min_[1] + bbox_max_[1]) * 0.5f;
	center_[2] = (bbox_min_[2] + bbox_max_[2]) * 0.5f;

	ccenter[0] /= (planes_.size() * 3);
	ccenter[1] /= (planes_.size() * 3);
	ccenter[2] /= (planes_.size() * 3);

	cout << center_[0] << " " << center_[1] << " " << center_[2] << endl;
	
	for(auto plane = planes_.begin(); plane != planes_.end(); plane++)
	{
		if(dot(ccenter, (*plane).n) + (*plane).d > 0.0f)
		{
			(*plane).n[0] *= -1.0f;
			(*plane).n[1] *= -1.0f;
			(*plane).n[2] *= -1.0f;
			(*plane).d *= -1.0f;
		}
	}

	planes_update_needed_ = false;
	freeze_texture_ = false;
	selected_ = false;

}

Brush::Brush(const Brush& brush_to_copy) {

	sides_ = brush_to_copy.sides_;
	planes_ = brush_to_copy.planes_;
	polygons_ = brush_to_copy.polygons_;

	copy(brush_to_copy.bbox_max_, brush_to_copy.bbox_max_ + 3, bbox_max_);
	copy(brush_to_copy.bbox_min_, brush_to_copy.bbox_min_ + 3, bbox_min_);
	copy(brush_to_copy.center_, brush_to_copy.center_ + 3, center_);

	planes_update_needed_ = false;
	freeze_texture_ = brush_to_copy.freeze_texture_;
	selected_ = false;
}

Brush::~Brush(){
}

void Brush::InitializeMesh(){

	polygons_.clear();
	polygons_.resize(planes_.size());

	for(auto polygon = polygons_.begin(); polygon != polygons_.end(); polygon++)
	{
		(*polygon).points.reserve(MAX_POLY_POINTS * 3);
	}
	

	for(auto plane1 = planes_.begin(); plane1 != planes_.end() - 2; plane1++)
	{
		for(auto plane2 = planes_.begin(); plane2 != planes_.end() - 1; plane2++)
		{
			for(auto plane3 = planes_.begin(); plane3 != planes_.end(); plane3++)
			{
				//if((i < j) && (j < k))
				if((((plane1 - planes_.begin()) < (plane2 - planes_.begin()))) &&
					(((plane2 - planes_.begin()) < (plane3 - planes_.begin()))))
				{
					float new_vtx_pos[3];
					bool legal = GetIntersection(*plane1, *plane2, *plane3, new_vtx_pos);


					if(legal)
					{
						for(auto plane = planes_.begin(); plane != planes_.end(); plane++)
						{
							if(((plane - planes_.begin()) != (plane1 - planes_.begin())) &&
								((plane - planes_.begin()) != (plane2 - planes_.begin())) &&
								((plane - planes_.begin()) != (plane3 - planes_.begin())))
							{
								if(dot((*plane).n, new_vtx_pos) + (*plane).d > epsilon)
								{
									legal = false;
									break;
								}
							}
						}
					}


					if(legal)
					{
						
						bool unique = true;

						for(auto point = polygons_[(plane1 - planes_.begin())].points.begin(); point != polygons_[(plane1 - planes_.begin())].points.end(); point +=3)
						{
							if(abs(point[0] - new_vtx_pos[0]) < epsilon)
							{
								if(abs(point[1] - new_vtx_pos[1]) < epsilon)
								{
									if(abs(point[2] - new_vtx_pos[2]) < epsilon)
									{
										unique = false;
										break;
									}
								}
							}
						}

						if(unique)
						{
							polygons_[(plane1 - planes_.begin())].points.insert(polygons_[(plane1 - planes_.begin())].points.end(), new_vtx_pos, new_vtx_pos + 3);
						}

						unique = true;

						for(auto point = polygons_[(plane2 - planes_.begin())].points.begin(); point != polygons_[(plane2 - planes_.begin())].points.end(); point +=3)
						{
							if(abs(point[0] - new_vtx_pos[0]) < epsilon)
							{
								if(abs(point[1] - new_vtx_pos[1]) < epsilon)
								{
									if(abs(point[2] - new_vtx_pos[2]) < epsilon)
									{
										unique = false;
										break;
									}
								}
							}
						}

						if(unique)
						{
							polygons_[(plane2 - planes_.begin())].points.insert(polygons_[(plane2 - planes_.begin())].points.end(), new_vtx_pos, new_vtx_pos + 3);
						}

						unique = true;

						for(auto point = polygons_[(plane3 - planes_.begin())].points.begin(); point != polygons_[(plane3 - planes_.begin())].points.end(); point +=3)
						{
							if(abs(point[0] - new_vtx_pos[0]) < epsilon)
							{
								if(abs(point[1] - new_vtx_pos[1]) < epsilon)
								{
									if(abs(point[2] - new_vtx_pos[2]) < epsilon)
									{
										unique = false;
										break;
									}
								}
							}
						}

						if(unique)
						{
							polygons_[(plane3 - planes_.begin())].points.insert(polygons_[(plane3 - planes_.begin())].points.end(), new_vtx_pos, new_vtx_pos + 3);
						}

					}
				}
			}
		}
	}


	/*cout << "main cycle done in:		" << t/1000 << " secs" << endl;
	cout << " intersection find time:	" << t2/1000 << " secs" << endl;
	cout << " checks1 time:				" << t4/1000 << " secs" << endl;
	cout << " checks2 time:				" << t3/1000 << " secs" << endl;*/

	for(auto polygon = polygons_.begin(); polygon != polygons_.end(); polygon++)
	{
		(*polygon).points.shrink_to_fit();
	}

	for(auto polygon = polygons_.begin(); polygon != polygons_.end(); polygon++)
	{
		if((*polygon).points.size() < 9)
		{
			//int a = (*polygon).points.size();
			int index = polygon - polygons_.begin();
			polygons_.erase(polygon);
			planes_.erase(planes_.begin() + index);
			polygon = polygons_.begin();
		}
	}

	for(auto polygon = polygons_.begin(); polygon != polygons_.end(); polygon++)
	{
		float poly_center[3] = {0.0, 0.0, 0.0};
		int num_points = (*polygon).points.size() / 3;
		for(auto point = (*polygon).points.begin(); point != (*polygon).points.end(); point += 3)
		{
			poly_center[0] += point[0];
			poly_center[1] += point[1];
			poly_center[2] += point[2];
		}
		float mult = 1.0f/(float)(num_points);
		poly_center[0] *= mult;
		poly_center[1] *= mult;
		poly_center[2] *= mult;

		for(auto vertex = (*polygon).points.begin(); vertex != (*polygon).points.end() - 6; vertex += 3)
		{
			float a[3] = {	vertex[0] - poly_center[0],
							vertex[1] - poly_center[1],
							vertex[2] - poly_center[2]};
			Normalize(a);
			float v[3];
			float d;

			Cross(v, planes_[polygon - polygons_.begin()].n, a);
			Normalize(v);

			d = -dot(v, poly_center);

			float biggest_dot = -2;
			auto biggest = (*polygon).points.begin();

			for(auto m = vertex + 3; m != (*polygon).points.end(); m += 3)
			{
				float p[3] = {m[0], m[1], m[2]};
				if(dot(v, p) + d > 0)
				{
					float b[3] = {	p[0] - poly_center[0],
									p[1] - poly_center[1],
									p[2] - poly_center[2]};
					Normalize(b);

					float dot1 = dot(a, b);
					if(dot1 > biggest_dot)
					{
						biggest_dot = dot1;
						biggest = m;
					}
				}
			}

			if(biggest != vertex + 3)
			{
				float temp[3];
				temp[0] = (vertex + 3)[0];
				temp[1] = (vertex + 3)[1];
				temp[2] = (vertex + 3)[2];

				(vertex + 3)[0] = biggest[0];
				(vertex + 3)[1] = biggest[1];
				(vertex + 3)[2] = biggest[2];
				
				biggest[0] = temp[0];
				biggest[1] = temp[1];
				biggest[2] = temp[2];
			}
		}
		
	}

	//if(!sides_.empty())
	//{
		/*for(int i = 0; i < polygons_.size(); i++)
		{
			polygons_[i].tex_vec[0][0] = sides_[ i * 19 + 9 ];
			polygons_[i].tex_vec[0][1] = sides_[ i * 19 + 10 ];
			polygons_[i].tex_vec[0][2] = sides_[ i * 19 + 11 ];
			polygons_[i].tex_vec[0][3] = sides_[ i * 19 + 12 ];

			polygons_[i].tex_vec[1][0] = sides_[ i * 19 + 13 ];
			polygons_[i].tex_vec[1][1] = sides_[ i * 19 + 14 ];
			polygons_[i].tex_vec[1][2] = sides_[ i * 19 + 15 ];
			polygons_[i].tex_vec[1][3] = sides_[ i * 19 + 16 ];

			polygons_[i].tex_angle = sides_[ i * 19 + 17 ];
			polygons_[i].tex_id = static_cast<int>(sides_[ i * 19 + 18 ]);
		}
		sides_.clear();*/
		//sides_.shrink_to_fit();
	//}


	for(auto polygon = polygons_.begin(); polygon != polygons_.end(); polygon++)
	{
		if(polygon - polygons_.begin() >= planes_.size())
		{
			break;
		}

		const float* normal = planes_[ polygon - polygons_.begin() ].n;
		if(abs(normal[0]) > abs(normal[1]) && abs(normal[0]) > abs(normal[2]))
		{
			normal[0] > 0 ? (*polygon).smoothing_group = 0 : (*polygon).smoothing_group = 3;
		} else
		if(abs(normal[1]) > abs(normal[0]) && abs(normal[1]) > abs(normal[2]))
		{
			normal[1] > 0 ? (*polygon).smoothing_group = 1 : (*polygon).smoothing_group = 4;
		} else
		{
			normal[2] > 0 ? (*polygon).smoothing_group = 2 : (*polygon).smoothing_group = 5;
		}
	}
}

void Brush::UpdateUVs() {

	if(freeze_texture_)
	{
		return;
	}

	float rot_matrix[16];

	for(unsigned int i = 0; i < polygons_.size(); i++)
	{
		polygons_[i].uvs.clear();
		polygons_[i].uvs.reserve(2 * polygons_[i].points.size() / 3);

		const float* normal = planes_[i].n;

		float sum = abs(polygons_[i].tex_vec[0][0]) + abs(polygons_[i].tex_vec[0][1]) + abs(polygons_[i].tex_vec[0][2]);

		float mag_u = 1.0f, mag_v = 1.0f;
		if(sum > epsilon)
		{
			mag_u = sqrt(dot(polygons_[i].tex_vec[0], polygons_[i].tex_vec[0]));
			mag_v = sqrt(dot(polygons_[i].tex_vec[1], polygons_[i].tex_vec[1]));
		}

		//if(sum < epsilon)
		if(abs(normal[0]) >= abs(normal[1]) && abs(normal[0]) >= abs(normal[2]))
		{
			polygons_[i].tex_vec[0][0] = 0.0f;	polygons_[i].tex_vec[0][1] = 0.0f;	polygons_[i].tex_vec[0][2] = 1.0f;
			polygons_[i].tex_vec[1][0] = 0.0f;	polygons_[i].tex_vec[1][1] = 1.0f;	polygons_[i].tex_vec[1][2] = 0.0f;
		} else
		{
			if(abs(normal[1]) >= abs(normal[0]) && abs(normal[1]) >= abs(normal[2]))
			{
				polygons_[i].tex_vec[0][0] = 1.0f;	polygons_[i].tex_vec[0][1] = 0.0f;	polygons_[i].tex_vec[0][2] = 0.0f;
				polygons_[i].tex_vec[1][0] = 0.0f;	polygons_[i].tex_vec[1][1] = 0.0f;	polygons_[i].tex_vec[1][2] = 1.0f;
			} else
			{
				polygons_[i].tex_vec[0][0] = 1.0f;	polygons_[i].tex_vec[0][1] = 0.0f;	polygons_[i].tex_vec[0][2] = 0.0f;
				polygons_[i].tex_vec[1][0] = 0.0f;	polygons_[i].tex_vec[1][1] = 1.0f;	polygons_[i].tex_vec[1][2] = 0.0f;
			}
		}

		if(sum > epsilon)
		{
		polygons_[i].tex_vec[0][0] *= mag_u;
		polygons_[i].tex_vec[0][1] *= mag_u;
		polygons_[i].tex_vec[0][2] *= mag_u;

		polygons_[i].tex_vec[1][0] *= mag_v;
		polygons_[i].tex_vec[1][1] *= mag_v;
		polygons_[i].tex_vec[1][2] *= mag_v;
		}

		float axis[3];
		Cross(axis, &polygons_[i].tex_vec[0][0], &polygons_[i].tex_vec[1][0]);
		Normalize(axis);
		matrixSetIdentityM(rot_matrix);
		matrixSetRotateM(rot_matrix, polygons_[i].tex_angle, axis[0], axis[1], axis[2]);
		float new_vecs[2][3];

		MatVecMult(&polygons_[i].tex_vec[0][0], rot_matrix, &new_vecs[0][0]);
		MatVecMult(&polygons_[i].tex_vec[1][0], rot_matrix, &new_vecs[1][0]);
		
		copy(&new_vecs[0][0], &new_vecs[0][0] + 3, &polygons_[i].tex_vec[0][0]);
		copy(&new_vecs[1][0], &new_vecs[1][0] + 3, &polygons_[i].tex_vec[1][0]);


		for(auto point_it = polygons_[i].points.begin(); point_it != polygons_[i].points.end(); point_it += 3)
		{
			float u =	point_it[0] *polygons_[i].tex_vec[0][0] + point_it[1] *polygons_[i].tex_vec[0][1] +
						point_it[2] *polygons_[i].tex_vec[0][2] + polygons_[i].tex_vec[0][3];
			float v =	point_it[0] *polygons_[i].tex_vec[1][0] + point_it[1] *polygons_[i].tex_vec[1][1] +
						point_it[2] *polygons_[i].tex_vec[1][2] + polygons_[i].tex_vec[1][3];
			polygons_[i].uvs.push_back(u/512.0f);
			polygons_[i].uvs.push_back(v/512.0f);
		}

	}

}

void Brush::ClipToBrush(Brush& brush, bool clip_on_plane) {

	vector<Poly> new_polygons;
	vector<Plane> new_planes;
	new_polygons.reserve(planes_.size()*2);
	new_planes.reserve(planes_.size()*2);

	//for(unsigned int i = 0; i < planes_.size(); i++)
	for(auto plane = planes_.begin(); plane != planes_.end(); plane++)
	{

		vector<Poly> polys2add;
		//ClipPolyToList (planes_[i], polygons_[i], brush.planes(), 0, clip_on_plane).swap(polys2add);
		ClipPolyToList (*plane, polygons_[plane - planes_.begin()], brush.planes(), 0, clip_on_plane).swap(polys2add);

		polys2add.shrink_to_fit();
		new_polygons.insert(new_polygons.end(), polys2add.begin(), polys2add.end());

		for(unsigned int j = 0; j < polys2add.size(); j++)
		{
			new_planes.push_back(*plane);
		}

	}


	new_polygons.shrink_to_fit();
	new_planes.shrink_to_fit();

	polygons_.swap(new_polygons);
	planes_.swap(new_planes);

	//polygons_.assign(new_polygons.begin(), new_polygons.end());
	//planes_.assign(new_planes.begin(), new_planes.end());
}

void Brush::Draw(int col){

	for(unsigned int i = 0; i < planes_.size(); i++) 
	{
		//glUniform3f(g_render_state.current_program->uniform(COL_UNIFORM), color_table[col][0], color_table[col][1], color_table[col][2]);
		glUniform3f(Renderer::render_state->current_program->uniform(COL_UNIFORM), color_table[col][0], color_table[col][1], color_table[col][2]);

		glEnableVertexAttribArray(Renderer::render_state->current_program->attribute(POSITION_ATTRIBUTE));
		glVertexAttribPointer(Renderer::render_state->current_program->attribute(POSITION_ATTRIBUTE), 3, GL_FLOAT, GL_FALSE, 0, &polygons_[i].points[0]);

		glDrawArrays(GL_POLYGON/*GL_TRIANGLE_FAN*/, 0, polygons_[i].points.size() / 3);
	}

}

void Brush::DrawWTexture() {

	if(selected_)
	{
		glUniform3f(Renderer::render_state->current_program->uniform(COL_UNIFORM), 0.0f, 1.5f, 1.5f);
	} else
	{
		glUniform3f(Renderer::render_state->current_program->uniform(COL_UNIFORM), 1.0f, 1.0f, 1.0f);
	}

	for(unsigned int i = 0; i < planes_.size(); i++) 
	{
		if(!polygons_[i].legal)
		{
			glUniform3f(Renderer::render_state->current_program->uniform(COL_UNIFORM), 1.0f, 0.0f, 0.0f);
		} else
		{
			if(selected_)
			{
				glUniform3f(Renderer::render_state->current_program->uniform(COL_UNIFORM), 0.0f, 1.5f, 1.5f);
			} else
			{
				glUniform3f(Renderer::render_state->current_program->uniform(COL_UNIFORM), 1.0f, 1.0f, 1.0f);
			}
		}

		Renderer::SwitchTexture(DIFFUSEMAP_SLOT, *polygons_[i].tex);

		glEnableVertexAttribArray(Renderer::render_state->current_program->attribute(POSITION_ATTRIBUTE));
		glVertexAttribPointer(Renderer::render_state->current_program->attribute(POSITION_ATTRIBUTE), 3, GL_FLOAT, GL_FALSE, 0, &polygons_[i].points[0]);

		glEnableVertexAttribArray(Renderer::render_state->current_program->attribute(UV_ATTRIBUTE));
		glVertexAttribPointer(Renderer::render_state->current_program->attribute(UV_ATTRIBUTE), 2, GL_FLOAT, GL_FALSE, 0, &polygons_[i].uvs[0]);

		glDrawArrays(GL_POLYGON/*GL_TRIANGLE_FAN*/, 0, polygons_[i].points.size() / 3);

		glDisableVertexAttribArray(Renderer::render_state->current_program->attribute(UV_ATTRIBUTE));
		glDisableVertexAttribArray(Renderer::render_state->current_program->attribute(POSITION_ATTRIBUTE));
	}

	if(selected_)
	{
		glUniform3f(Renderer::render_state->current_program->uniform(COL_UNIFORM), 1.0f, 1.0f, 1.0f);
	}

}

void Brush::DrawVertices() {
	glUniform3f(Renderer::render_state->current_program->uniform(COL_UNIFORM), 1.0f, 1.0f, 1.0f);
	for(auto polygon = polygons_.begin(); polygon != polygons_.end(); polygon++)
	{
		glBegin(GL_POINTS);
		for(auto point = (*polygon).points.begin(); point != (*polygon).points.end(); point += 3)
		{
			glVertex3f(point[0], point[1], point[2]);
		}
		glEnd();
	}
}

void Brush::Move(const float vec[3]) {

	bbox_min_[0] += vec[0];	bbox_min_[1] += vec[1];	bbox_min_[2] += vec[2];
	bbox_max_[0] += vec[0];	bbox_max_[1] += vec[1];	bbox_max_[2] += vec[2];

	for(unsigned int i = 0; i < polygons_.size(); i++)
	{
		for(auto point = polygons_[i].points.begin(); point != polygons_[i].points.end(); point += 3)
		{
			point[0] += vec[0];		point[1] += vec[1];		point[2] += vec[2];
		}
		
		float ddot = dot(planes_[i].n, vec);
		planes_[i].d += ddot;
	}

}

void Brush::Rotate(const float axis[3], const float angle) {

	float mat[16];
	matrixSetIdentityM(mat);
	
	matrixRotateM(mat, angle, axis[0], axis[1], axis[2]);

	for(unsigned int i = 0; i < polygons_.size(); i++)
	{
		for(auto point = polygons_[i].points.begin(); point != polygons_[i].points.end(); point += 3)
		{
			point[0] -= center_[0];
			point[1] -= center_[1];
			point[2] -= center_[2];
			
			float new_point[3];
			MatVecMult(&point[0], mat, new_point);
			copy(new_point, new_point + 3, point);

			point[0] += center_[0];
			point[1] += center_[1];
			point[2] += center_[2];
		}

		float new_normal[3];
		MatVecMult(planes_[i].n, mat, new_normal);
		float dd = dot(planes_[i].n, new_normal);
		copy(new_normal, new_normal + 3, planes_[i].n);
		planes_[i].d *= dd;
	}

	UpdateBbox();

}

void Brush::Rotate(const float axis[3], const float angle, const float center[3]) {

	float move_vec[3] = {	-center[0] + center_[0],
							-center[1] + center_[1],
							-center[2] + center_[2] };
	Move(move_vec);
	Rotate(axis, angle);
	move_vec[0] = -move_vec[0];	move_vec[1] = -move_vec[1];	move_vec[2] = -move_vec[2];
	Move(move_vec);

}


void Brush::Rescale(const float old_box_min[3], const float old_box_max[3], const float new_box_min[3], const float new_box_max[3]) {

	float mult_vec[3] = {	(new_box_max[0]-new_box_min[0])/(old_box_max[0] - old_box_min[0]), 
							(new_box_max[1]-new_box_min[1])/(old_box_max[1] - old_box_min[1]), 
							(new_box_max[2]-new_box_min[2])/(old_box_max[2] - old_box_min[2]) };

	for(unsigned int i = 0; i < polygons_.size(); i++)
	{
		for(auto point = polygons_[i].points.begin(); point != polygons_[i].points.end(); point += 3)
		{
			point[0] -= old_box_min[0];		point[1] -= old_box_min[1];		point[2] -= old_box_min[2];
			point[0] *= mult_vec[0];		point[1] *= mult_vec[1];		point[2] *= mult_vec[2];
			point[0] += new_box_min[0];		point[1] += new_box_min[1];		point[2] += new_box_min[2];
		}
		
		float ddot = dot(planes_[i].n, old_box_min);
		float m = dot(planes_[i].n, mult_vec);
		planes_[i].d -= ddot;
		planes_[i].d *= m;
		ddot = dot(planes_[i].n, new_box_min);
		planes_[i].d += ddot;
	}

	for(unsigned int i = 0; i < polygons_.size(); i++)
	{
		vector< float >& points = polygons_[i].points;

		float v1[3] = {points[3] - points[0], points[4] - points[1], points[5] - points[2]};
		float v2[3] = {points[6] - points[0], points[7] - points[1], points[8] - points[2]};

		Cross(planes_[i].n, v1, v2);
		Normalize(planes_[i].n);
		planes_[i].d = -dot(planes_[i].n, &points[0]);
	}

	bbox_min_[0] -= old_box_min[0];		bbox_min_[1] -= old_box_min[1];		bbox_min_[2] -= old_box_min[2];
	bbox_min_[0] *= mult_vec[0];		bbox_min_[1] *= mult_vec[1];		bbox_min_[2] *= mult_vec[2];
	bbox_min_[0] += new_box_min[0];		bbox_min_[1] += new_box_min[1];		bbox_min_[2] += new_box_min[2];

	bbox_max_[0] -= old_box_min[0];		bbox_max_[1] -= old_box_min[1];		bbox_max_[2] -= old_box_min[2];
	bbox_max_[0] *= mult_vec[0];		bbox_max_[1] *= mult_vec[1];		bbox_max_[2] *= mult_vec[2];
	bbox_max_[0] += new_box_min[0];		bbox_max_[1] += new_box_min[1];		bbox_max_[2] += new_box_min[2];

	center_[0] = (bbox_min_[0] + bbox_max_[0]) * 0.5f;
	center_[1] = (bbox_min_[1] + bbox_max_[1]) * 0.5f;
	center_[2] = (bbox_min_[2] + bbox_max_[2]) * 0.5f;
}

bool Brush::TraceL(const float orig[3], const float dir[3], float* dist) {

	float min_dist = numeric_limits<float>::max();
	bool b = false;
	for(auto polygon = polygons_.begin(); polygon != polygons_.end(); polygon++)
	{
		auto point1 = (*polygon).points.begin();
		for(auto point2 = (*polygon).points.begin() + 3; point2 != (*polygon).points.end() - 3; point2 += 3)
		{
			auto point3 = point2 + 3;

			float tri[9];
			copy(point1, point1 + 3, tri);
			copy(point2, point2 + 3, tri + 3);
			copy(point3, point3 + 3, tri + 6);

			float d;
			if(TraceLine(orig, dir, tri, &d))
			{
				b = true;
				if(d < min_dist)
				{
					min_dist = d;
				}
			}

		}
	}

	*dist = min_dist;
	return b;

}

void Brush::UpdateBbox() {

	bbox_min_[0] = numeric_limits<float>::max();
	bbox_min_[1] = numeric_limits<float>::max();
	bbox_min_[2] = numeric_limits<float>::max();

	bbox_max_[0] = -numeric_limits<float>::max();
	bbox_max_[1] = -numeric_limits<float>::max();
	bbox_max_[2] = -numeric_limits<float>::max();

	for(auto polygon = polygons_.begin(); polygon != polygons_.end(); polygon++)
	{
		for(auto point = (*polygon).points.begin(); point != (*polygon).points.end(); point += 3)
		{
			bbox_min_[0] = min(bbox_min_[0], point[0]);
			bbox_min_[1] = min(bbox_min_[1], point[1]);
			bbox_min_[2] = min(bbox_min_[2], point[2]);

			bbox_max_[0] = max(bbox_max_[0], point[0]);
			bbox_max_[1] = max(bbox_max_[1], point[1]);
			bbox_max_[2] = max(bbox_max_[2], point[2]);
		}
	}

	center_[0] = (bbox_min_[0] + bbox_max_[0]) * 0.5f;
	center_[1] = (bbox_min_[1] + bbox_max_[1]) * 0.5f;
	center_[2] = (bbox_min_[2] + bbox_max_[2]) * 0.5f;

}

bool Brush::SelectPoints(float x, float y, const float* view_proj_mat, const float* dims, const int* dims_px) {

	p_selected_points_.clear();

	for(auto polygon = polygons_.begin(); polygon != polygons_.end(); polygon++)
	{
		for(auto point = (*polygon).points.begin(); point != (*polygon).points.end(); point += 3)
		{
			float proj_point[3];
			MatVecMult(&point[0], view_proj_mat, proj_point);

			float vec2[] = {	(0.5f * (proj_point[0] - x) / dims[2]) * dims_px[2],
								(0.5f * (proj_point[1] - y) / dims[3]) * dims_px[3]	};

			float l_sqr = vec2[0] * vec2[0] + vec2[1] * vec2[1];
			if(l_sqr < 16.0f)
			{
				p_selected_points_.push_back(&point[0]);
			}
		}
	}

	return !p_selected_points_.empty();

}

bool Brush::SelectFace(const float orig[3], const float dir[3], float* dist, Poly** face) {

	float min_dist = numeric_limits<float>::max();
	Poly* closest_face = nullptr;
	bool b = false;
	for(auto polygon = polygons_.begin(); polygon != polygons_.end(); polygon++)
	{
		auto point1 = (*polygon).points.begin();
		for(auto point2 = (*polygon).points.begin() + 3; point2 != (*polygon).points.end() - 3; point2 += 3)
		{
			auto point3 = point2 + 3;

			float tri[9];
			copy(point1, point1 + 3, tri);
			copy(point2, point2 + 3, tri + 3);
			copy(point3, point3 + 3, tri + 6);

			float d;
			if(TraceLine(orig, dir, tri, &d))
			{
				b = true;
				if(d < min_dist)
				{
					min_dist = d;
					closest_face = &(*polygon);
				}
			}

		}
	}

	*dist = min_dist;
	*face = closest_face;
	return b;

}

void Brush::MoveSelected(const float vec[3], int grid_size) {

	if(p_selected_points_.empty())
	{
		return;
	}
	
	for(auto p_point = p_selected_points_.begin(); p_point != p_selected_points_.end(); p_point += 1)
	{
		(*p_point)[0] += vec[0];
		(*p_point)[1] += vec[1];
		(*p_point)[2] += vec[2];

		(*p_point)[0] = Round((*p_point)[0] / (float)grid_size) * (float)grid_size;
		(*p_point)[1] = Round((*p_point)[1] / (float)grid_size) * (float)grid_size;
		(*p_point)[2] = Round((*p_point)[2] / (float)grid_size) * (float)grid_size;
	}
	planes_update_needed_ = true;
}

void Brush::RebuildFromPolys() {

	if(!planes_update_needed_)
	{
		return;
	}
	/*
	for(int i = 0; i < polygons_.size(); i++)
	{
		auto points = polygons_[i].points;

		float v1[3] = {points[3] - points[0], points[4] - points[1], points[5] - points[2]};
		float v2[3] = {points[6] - points[0], points[7] - points[1], points[8] - points[2]};
		cross(planes_[ i ].n, v1, v2);
		Normalize(planes_[ i ].n);
		planes_[ i ].d = -dot(planes_[ i ].n, points);
	}*/

	vector< float > old_tex_vecs; 
	old_tex_vecs.reserve(polygons_.size() * 8 + 32);
	//vector<unsigned int> old_tex_ids;
	vector<shared_ptr<GLTexture>> old_tex_ids;
	old_tex_ids.reserve(polygons_.size());

	vector< float > old_tex_angles; 

	planes_.clear();
	for(unsigned int i = 0; i < polygons_.size(); i++)
	{
		for(auto point1 = polygons_[i].points.begin() + 3; point1 != polygons_[i].points.end() - 3; point1 += 3)
		{
			float* point2;
			point2 = &point1[3];

			float v1[3] = {point1[0] - polygons_[i].points[0], point1[1] - polygons_[i].points[1], point1[2] - polygons_[i].points[2]};
			float v2[3] = {point2[0] - polygons_[i].points[0], point2[1] - polygons_[i].points[1], point2[2] - polygons_[i].points[2]};

			if(dot(v1, v1) < epsilon || dot(v2, v2) < epsilon)
			{
				continue;
			} else
			{
				float v3[3] = {point2[0] - point1[0], point2[1] - point1[1], point2[2] - point1[2]};
				if(dot(v3, v3) < epsilon)
				{
					continue;
				}
			}

			Plane new_plane;
			Cross(new_plane.n, v1, v2);
			Normalize(new_plane.n);
			new_plane.d = -dot(new_plane.n, point2);

			bool unique = true;
			if(!planes_.empty())
			for(auto plane = planes_.begin(); plane != planes_.end(); plane++)
			{
				if(abs((*plane).d - new_plane.d) < epsilon)
				{
					if(abs((*plane).n[0] - new_plane.n[0]) < epsilon)
						if(abs((*plane).n[1] - new_plane.n[1]) < epsilon)
							if(abs((*plane).n[2] - new_plane.n[2]) < epsilon)
							{
								unique = false;
								break;
							}
					
				}
			}

			if(unique)
			{
				planes_.push_back(new_plane);
				old_tex_vecs.insert(old_tex_vecs.end(), &polygons_[i].tex_vec[0][0], &polygons_[i].tex_vec[0][0] + 8);
				old_tex_angles.push_back(polygons_[i].tex_angle);
				old_tex_ids.push_back(polygons_[i].tex);
			}
		}
	}

	bool convex = true;
	float ccenter[3] = { 0.0f, 0.0f, 0.0f };
	int num_points = 0;

	for(auto plane_it = planes_.begin(); plane_it != planes_.end(); plane_it++)
	{
		for(unsigned int i = 0; i < polygons_.size(); i++)
		{
			for(auto point1 = polygons_[i].points.begin(); point1 != polygons_[i].points.end(); point1 += 3)
			{

				if(dot(&point1[0], (*plane_it).n) + (*plane_it).d > epsilon)
				{

					convex = false;

				}
			}
		}
	}

	if(!convex)
	{
		//int num_points = 0;
		for(unsigned int i = 0; i < polygons_.size(); i++)
		{
			for(auto point1 = polygons_[i].points.begin(); point1 != polygons_[i].points.end(); point1 += 3)
			{
				for(auto plane_it = planes_.begin(); plane_it != planes_.end(); plane_it++)
				{
					if(dot(&point1[0], (*plane_it).n) + (*plane_it).d < 0.0f)
					{
						ccenter[0] += point1[0];
						ccenter[1] += point1[1];
						ccenter[2] += point1[2];
						num_points++;
					}
				}
			}
		}

		ccenter[0] /= num_points;
		ccenter[1] /= num_points;
		ccenter[2] /= num_points;

		for(auto plane = planes_.begin(); plane != planes_.end(); plane++)
		{
			if(dot(ccenter, (*plane).n) + (*plane).d > epsilon)
			{
				(*plane).n[0] *= -1.0f;
				(*plane).n[1] *= -1.0f;
				(*plane).n[2] *= -1.0f;
				(*plane).d *= -1.0f;
			}
		}
	}

	InitializeMesh();

	for(unsigned int i = 0; i < polygons_.size(); i++)
	{
		//if(i >= old_tex_vecs.size()/8)
		if(i >= old_tex_angles.size())
		{
			break;
		}
		copy(&old_tex_vecs[i*8], &old_tex_vecs[i*8] + 8, &polygons_[i].tex_vec[0][0]);
		polygons_[i].tex_angle = old_tex_angles[i];
		polygons_[i].tex = old_tex_ids[i];
	}

	bool b = freeze_texture_;
	freeze_texture_ = false;
	UpdateUVs();
	freeze_texture_ = b;
	UpdateBbox();

	if(polygons_.size() != planes_.size())
	{
		system("pause");
	}
}

void Brush::ScaleTexture(float ds){
	int axis;
	ds < 0 ? axis = 1 : axis = 0;
	for(auto poly = polygons_.begin(); poly != polygons_.end(); poly++)
	{
		(*poly).tex_vec[axis][0] *= fabs(ds);
		(*poly).tex_vec[axis][1] *= fabs(ds);
		(*poly).tex_vec[axis][2] *= fabs(ds);
	}
}

void Brush::SetTexture(std::shared_ptr<GLTexture> tex) {

	for(auto poly = polygons_.begin(); poly != polygons_.end(); poly++)
	{
		(*poly).tex = tex;
	}

}

void Brush::ResetTexture() {

	for(auto poly = polygons_.begin(); poly != polygons_.end(); poly++)
	{
		(*poly).tex_vec[0][0] = (*poly).tex_vec[0][1] = (*poly).tex_vec[0][2] = (*poly).tex_vec[0][3] = 0.0f;
		(*poly).tex_vec[1][0] = (*poly).tex_vec[1][1] = (*poly).tex_vec[1][2] = (*poly).tex_vec[1][3] = 0.0f;
		(*poly).tex_angle = 0.0f;
	}

}

void Brush::GetUsedTextures(std::set<std::shared_ptr<GLTexture>>& list){

	for(auto poly_it = polygons_.begin(); poly_it != polygons_.end(); poly_it++)
	{
		list.insert((*poly_it).tex);
	}

}

/*void Brush::ReplaceTexture(const unsigned int old_id, const unsigned int new_id){
	for(auto poly_it = polygons_.begin(); poly_it != polygons_.end(); poly_it++)
	{
		if((*poly_it).tex_id == old_id)
		{
			(*poly_it).tex_id = new_id;
		}
	}
}*/

void Brush::SetTexVec(vector<float>::iterator& vec, std::vector<std::string>& tex_names) {

	if(!polygons_.empty())
	{
		for(auto poly = polygons_.begin(); poly != polygons_.end(); poly++)
		{
			copy(vec, vec + 8, &(*poly).tex_vec[0][0]);
			(*poly).tex_angle = vec[8];
			//(*poly).tex_id = static_cast<int>(vec[9]);
			(*poly).tex = Renderer::LoadTexture2(tex_names[static_cast<int>(vec[9])].c_str());
			(*poly).smoothing_group = static_cast<int>(vec[10]);
			//cout << "uv: " << vec[0] << " " << vec[1] << " " << vec[2] << " " << vec[3] << " " << vec[4] << endl;
			//copy(vec, vec + 4, (*poly).tex_vec);
			vec += 11;
		}
	}

}

bool GetIntersection(Plane& pl1, Plane& pl2, Plane& pl3, float* p){

	float cross_2_3[3];
	Cross(cross_2_3, pl2.n, pl3.n);

	float denom = dot(pl1.n, cross_2_3);

	if(abs(denom) < epsilon)
	{
		return false;
	}
	denom = 1/denom;

	float cross_3_1[3];
	Cross(cross_3_1, pl3.n, pl1.n);

	float cross_1_2[3];
	Cross(cross_1_2, pl1.n, pl2.n);

	p[0] = (-pl1.d * cross_2_3[0] -pl2.d * cross_3_1[0] -pl3.d * cross_1_2[0]) * denom;
	p[1] = (-pl1.d * cross_2_3[1] -pl2.d * cross_3_1[1] -pl3.d * cross_1_2[1]) * denom;
	p[2] = (-pl1.d * cross_2_3[2] -pl2.d * cross_3_1[2] -pl3.d * cross_1_2[2]) * denom;

	return true;
}
/*
int ClassifyPoint (Plane& plane, float point[3]) {

	float result = plane.n[0] * point[0] + plane.n[1] * point[1] + plane.n[2] * point[2] + plane.d;

	if(result > epsilon)
	{
		return FRONT;
	} else
		if(result < -epsilon)
		{
			return BACK;
		}

	return ONPLANE;
}

int ClassifyPolygon (Plane& plane, Poly& poly) {

	int num_front = 0, num_back = 0, num_on_plane = 0;

	int num_points = poly.points.size() / 3;

	for(auto point = poly.points.begin(); point != poly.points.end(); point += 3)
	{
		float result = plane.n[0] * point[0] + plane.n[1] * point[1] + plane.n[2] * point[2] + plane.d;
		if(result > epsilon)
		{
			num_front++;
		} else
			if(result < -epsilon)
			{
				num_back++;
			} else
			{
				num_on_plane++;
			}
	}


	if(num_front == num_points)
	{
		return FRONT;
	}
	
	if(num_back == num_points)
	{
		return BACK;
	}

	if(num_on_plane == num_points)
	{
		return ONPLANE;
	}

	return SPANNING;

}


int ClassifyPolygon2 (Plane& plane, Poly& poly) {

	int num_front = 0, num_back = 0, num_on_plane = 0;

	int num_points = poly.points.size() / 3;
	//for(int i = 0; i < num_points; i++)
	for(auto point = poly.points.begin(); point != poly.points.end(); point += 3)
	{
		float result = plane.n[0] * point[0] + plane.n[1] * point[1] + plane.n[2] * point[2] + plane.d;

		if(result > epsilon)
		{
			num_front++;
		} else
			if(result < -epsilon)
			{
				num_back++;
			} else
			{
				num_on_plane++;
			}
	}

	if(num_front == num_points)
	{
		return FRONT;
	}
	

	if(num_back == num_points)
	{
		return BACK;
	}

	if(num_on_plane == num_points)
	{
		return ONPLANE;
	}

	if(num_front + num_on_plane == num_points)
	{
		return TOUCH_FRONT;
	}

	if(num_back + num_on_plane == num_points)
	{
		return TOUCH_BACK;
	}

	return SPANNING;

}

*/

vector<Poly> ClipPolyToList (Plane& poly_plane, Poly& poly, vector<Plane>& clippers, unsigned int clipper_index, bool clip_on_plane) {

	vector<Poly> ret_val(0);

	float angle;

	//cout << "here" << endl;

	switch(ClassifyPolygon2(clippers[clipper_index], poly))
	{
	case TOUCH_FRONT:
	case FRONT:
		//cout << "its in front" << endl;
		ret_val.push_back(poly);
		//cout << "returning from FRONT" << endl;
		return ret_val;
	case TOUCH_BACK:
	case BACK:
		//cout << "its in back" << endl;
		if(clipper_index == clippers.size() - 1)
		{
			return ret_val; //NULL
		}
		//cout << "call ClipPolyToList from case BACK" << endl;

		ClipPolyToList (poly_plane, poly, clippers, clipper_index + 1, clip_on_plane).swap(ret_val);
		return ret_val;
	case ONPLANE:
		//cout << "its on plane" << endl;
		angle = dot(poly_plane.n, clippers[clipper_index].n) - 1.0f;

		if((angle > -epsilon) && (angle < epsilon))
		{
			if(!clip_on_plane)
			{
				ret_val.push_back(poly);
				//cout << "returning from ONPLANE" << endl;
				return ret_val;
			}
		}

		if(clipper_index == clippers.size() - 1)
		{
			//cout << "returning from ONPLANE 2" << endl;
			return ret_val; //NULL
		}

		ClipPolyToList (poly_plane, poly, clippers, clipper_index + 1, clip_on_plane).swap(ret_val);
		return ret_val;
	case SPANNING:
		
			//cout << "here1" << endl;
			Poly front;
			Poly back;

			vector<Poly> front_list;
			vector<Poly> back_list;

			front_list.reserve(8);
			back_list.reserve(8);
			//cout << "here22" << endl;
			SplitPoly(poly, clippers[clipper_index], front, back);

			//cout << "num_front2: " << front.points.size() << endl;
			//cout << "num_back2: " << back.points.size() << endl;

			front_list.push_back(front);
			back_list.push_back(back);
			//cout << "here33" << endl;
			if(clipper_index == clippers.size() - 1)
			{
				return front_list;
			}
			//cout << back.points.size() << endl;
			//cout << "call ClipPolyToList from case SPANNING" << endl;


			vector<Poly> back_frags;
			ClipPolyToList (poly_plane, back, clippers, clipper_index + 1, clip_on_plane).swap(back_frags);


			//cout << "here44" << endl;
			if(back_frags.size() == 0)
			{
				//cout << "returning front_list" << endl;
				return front_list;
			}

			if(back_frags.size() == back_list.size())
				if(equal(back_frags.begin(), back_frags.end(), back_list.begin()))
				{
					ret_val.push_back(poly);
					return ret_val;
				}

			front_list.insert(front_list.end(), back_frags.begin(), back_frags.end());
			return front_list;
			
	}
	

	cout << "error ..." << endl;
	return ret_val;

}


vector<Poly> ClipPolyToList2 (Plane& poly_plane, Poly& poly, vector<Plane>& clippers, vector<Poly>& clipper_polys, unsigned int clipper_index, bool clip_on_plane) {

	vector<Poly> ret_val(0);
	vector<Poly> ret_val1(0);
	vector<Plane> new_clippers;
	//float angle;

	//cout << "here" << endl;

	switch(ClassifyPolygon2(clippers[clipper_index], poly))
	{
	case ONPLANE:
		//if(clipper_index == clippers.size() - 1)
		//{
			//ret_val.push_back(poly);

		//	return ret_val;
		//}

		
		/*
		for(int i = 0; i < clipper_polys[clipper_index].points.size()/3; i++)
		{
			Plane new_plane;
			float* p1 = &clipper_polys[clipper_index].points[i* 3];
			float* p2;
			if(i == clipper_polys[clipper_index].points.size()/3 - 1)
			{
				p2 = &clipper_polys[clipper_index].points[(0)* 3];
			} else
			{
				p2 = &clipper_polys[clipper_index].points[(i + 1)* 3];
			}
			float v[3] = {	-p2[0] + p1[0],
							-p2[1] + p1[1],
							-p2[2] + p1[2]};
			Normalize(v);

			//float normal[3];
			Cross(new_plane.n, v, poly_plane.n);
			Normalize(new_plane.n);

			new_plane.d = -dot(new_plane.n, p1);
			
			new_clippers.push_back(new_plane);
		}
		
		ClipPolyToList2 (poly_plane, poly, new_clippers, vector<Poly>(), 0, clip_on_plane).swap(ret_val1);
		//cout << "111:    " << ret_val1.size() << endl;
		for(unsigned int i = 0; i < ret_val1.size(); i++)
		{
			vector<Poly> add;
			ClipPolyToList2 (poly_plane, poly, clippers, clipper_polys, clipper_index + 1, clip_on_plane).swap(add);
			ret_val.insert(ret_val.end(), add.begin(), add.end());
		}
		
		return ret_val;
		*/

		//ClipPolyToList2 (poly_plane, poly, clippers, clipper_polys, clipper_index + 1, clip_on_plane).swap(ret_val);
		//return ret_val;
	case TOUCH_FRONT:
	case FRONT:
		if(clipper_index == clippers.size() - 1)
		{
			ret_val.push_back(poly);

			return ret_val;
		}
		ClipPolyToList2 (poly_plane, poly, clippers, clipper_polys, clipper_index + 1, clip_on_plane).swap(ret_val);
		return ret_val;
	case TOUCH_BACK:
	case BACK:
		//cout << "its in back" << endl;
		//if(clipper_index == clippers.size() - 1)
		//{
		//	return ret_val; //NULL
		//}
		//cout << "call ClipPolyToList from case BACK" << endl;

		//ClipPolyToList2 (poly_plane, poly, clippers, clipper_index + 1, clip_on_plane).swap(ret_val);
		return ret_val;
	case SPANNING:
		
			//cout << "here1" << endl;
			Poly front;
			Poly back;

			vector<Poly> front_list;
			vector<Poly> back_list;

			front_list.reserve(8);
			back_list.reserve(8);
			//cout << "here22" << endl;
			SplitPoly(poly, clippers[clipper_index], front, back);

			//cout << "num_front2: " << front.points.size() << endl;
			//cout << "num_back2: " << back.points.size() << endl;

			front_list.push_back(front);
			back_list.push_back(back);
			//cout << "here33" << endl;
			if(clipper_index == clippers.size() - 1)
			{
				return front_list;//
			}
			//cout << back.points.size() << endl;
			//cout << "call ClipPolyToList from case SPANNING" << endl;


			vector<Poly> back_frags;
			ClipPolyToList2 (poly_plane, front, clippers, clipper_polys, clipper_index + 1, clip_on_plane).swap(back_frags);


			/*if(back_frags.size() == front_list.size())
				if(equal(back_frags.begin(), back_frags.end(), front_list.begin()))
				{
					ret_val.push_back(poly);
					return ret_val;
				}
				*/
			return back_frags;
			
	}
	

	cout << "error ..." << endl;
	return ret_val;

}


void SplitPoly(Poly& poly, Plane& plane, Poly& front, Poly& back) {
	
	int num_points = poly.points.size() / 3;

	//int* locations = new int[num_points];
	int locations[MAX_POLY_POINTS];

	//front.tex_id = back.tex_id = poly.tex_id;
	front.tex = back.tex = poly.tex;
	front.tex_angle = back.tex_angle = poly.tex_angle;
	front.smoothing_group = back.smoothing_group = poly.smoothing_group;
	copy(&poly.tex_vec[0][0], &poly.tex_vec[0][0] + 8, &front.tex_vec[0][0]);
	copy(&poly.tex_vec[0][0], &poly.tex_vec[0][0] + 8, &back.tex_vec[0][0]);

	for(int i = 0; i < num_points; i++)
	{
		float res = poly.points[i*3] * plane.n[0] + poly.points[i*3 + 1] * plane.n[1] + poly.points[i*3 + 2] * plane.n[2] + plane.d;
		if(res > epsilon)
		{
			locations[i] = FRONT;
		} else
			if(res < -epsilon)
			{
				locations[i] = BACK;
			} else
			{
				locations[i] = ONPLANE;
			}
	}

	front.points.reserve(MAX_POLY_POINTS * 3);
	back.points.reserve(MAX_POLY_POINTS * 3);

	for(int i = 0; i < num_points; i++)
	{
		int m = i + 1;

		bool ignore = false;

		if(i == num_points - 1)
		{
			m = 0;
		}

		switch(locations[i])
		{
		case FRONT:
			front.points.insert(front.points.end(), &poly.points[i*3], &poly.points[i*3] + 3);
			break;
		case BACK:
			back.points.insert(back.points.end(), &poly.points[i*3], &poly.points[i*3] + 3);
			break;
		case ONPLANE:
			front.points.insert(front.points.end(), &poly.points[i*3], &poly.points[i*3] + 3);
			back.points.insert(back.points.end(), &poly.points[i*3], &poly.points[i*3] + 3);
			break;
		}

		if ((locations[i] == ONPLANE) && (locations[m] != ONPLANE))
		{
			ignore = true;
		} else
			if ((locations[i] != ONPLANE) && (locations[m] == ONPLANE))
			{
				ignore = true;
			}

		if((!ignore) && (locations[i] != locations[m]))
		{
			float edge_dir[3] = {	poly.points[i*3] - poly.points[m*3],
									poly.points[i*3 + 1] - poly.points[m*3 + 1],
									poly.points[i*3 + 2] - poly.points[m*3 + 2]};
			//normalize(edge_dir[0], edge_dir[1], edge_dir[2]);
			Normalize(edge_dir);

			float denom = dot(plane.n, edge_dir);

			if((denom < epsilon) && (denom > -epsilon))
			{
				continue;
			}
			float* pPoint = &poly.points[i*3];
			float t = -(dot(plane.n, pPoint) + plane.d) / denom;

			float new_vertices[3] = {	pPoint[0] + t * edge_dir[0],
										pPoint[1] + t * edge_dir[1],
										pPoint[2] + t * edge_dir[2] };

			front.points.insert(front.points.end(), &new_vertices[0], &new_vertices[0] + 3);
			back.points.insert(back.points.end(), &new_vertices[0], &new_vertices[0] + 3);

		}

	}

	//delete[] locations;


	front.points.shrink_to_fit();
	back.points.shrink_to_fit();

}


Brush* CSGUnion(Brush* brushes, int num_brushes){

	//Brush* clipped_brushes = (Brush*)operator new(sizeof(Brush) * num_brushes);
	Brush* clipped_brushes = new Brush[num_brushes];

	for(int i = 0; i < num_brushes; i++)
	{
		//new(&clipped_brushes[i])Brush(brushes[i]);
		clipped_brushes[i] = Brush(brushes[i]);
	}

	bool clip_on_plane;
	
	for(int i = 0; i < num_brushes; i++)
	{
		clip_on_plane = false;

		for(int j = 0; j < num_brushes; j++)
		{
			if(i != j)
			{
				clipped_brushes[i].ClipToBrush(brushes[j], clip_on_plane);
			} else
			{
				clip_on_plane = true;
			}
		}
	}
	
	
	/*if(brushes){
		for(int i = num_brushes-1; i >= 0; i--){
			brushes[i].~Brush();
		}
		operator delete[](brushes);
	}*/
	delete[] brushes;

	return clipped_brushes;
}


Poly outside_polys[6];
Plane outside_planes[6];

vector<Poly> total_polygon_list;
vector<Plane> total_planes_list;

BSPNode* root_node;
vector<BSPNode*> nodes_array;

vector<Portal*> g_portals;

int best_div_poly;
vector<Poly> g_front_poly_list, g_back_poly_list;
vector<Plane> g_front_plane_list, g_back_plane_list;

vector<Poly> g_prev_dividers;

float g_max_dist;


void NewFunc(Brush* brushes, int num_brushes) {

	float	bbox_min[3] = {9e29f, 9e29f, 9e29f},
			bbox_max[3] = {-9e29f, -9e29f, -9e29f};

	for(int i = 0; i < num_brushes; i++)
	{
		for(unsigned int j = 0; j < brushes[i].planes().size(); j++)
		{
			int num_points = brushes[i].polygons()[j].points.size() / 3;
			for(int p = 0; p < num_points; p++)
			{
				float* point = &brushes[i].polygons()[j].points[p * 3];

				if(point[0] < bbox_min[0])
					bbox_min[0] = point[0];
				if(point[1] < bbox_min[1])
					bbox_min[1] = point[1];
				if(point[2] < bbox_min[2])
					bbox_min[2] = point[2];

				if(point[0] > bbox_max[0])
					bbox_max[0] = point[0];
				if(point[1] > bbox_max[1])
					bbox_max[1] = point[1];
				if(point[2] > bbox_max[2])
					bbox_max[2] = point[2];
			}
		}
	}

	cout << "bbox_min: " << bbox_min[0] << " " << bbox_min[1] << " " << bbox_min[2] << endl;
	cout << "bbox_max: " << bbox_max[0] << " " << bbox_max[1] << " " << bbox_max[2] << endl;

	bbox_min[0] -= 10.0;
	bbox_min[1] -= 10.0;
	bbox_min[2] -= 10.0;

	bbox_max[0] += 10.0;
	bbox_max[1] += 10.0;
	bbox_max[2] += 10.0;


	g_max_dist = sqrt((bbox_max[0] - bbox_min[0])*(bbox_max[0] - bbox_min[0]) + (bbox_max[1] - bbox_min[1]) * (bbox_max[1] - bbox_min[1]) + (bbox_max[2] - bbox_min[2])*(bbox_max[2] - bbox_min[2]));

	/***********************************************/

	outside_planes[0].n[0] = 1.0;
	outside_planes[0].n[1] = 0.0;
	outside_planes[0].n[2] = 0.0;
	outside_planes[0].d = -bbox_min[0];

	outside_polys[0].legal = false;
	outside_polys[0].points.resize(4*3);
	outside_polys[0].points[0] = bbox_min[0];
	outside_polys[0].points[1] = bbox_min[1];
	outside_polys[0].points[2] = bbox_min[2];

	outside_polys[0].points[3] = bbox_min[0];
	outside_polys[0].points[4] = bbox_max[1];
	outside_polys[0].points[5] = bbox_min[2];

	outside_polys[0].points[6] = bbox_min[0];
	outside_polys[0].points[7] = bbox_max[1];
	outside_polys[0].points[8] = bbox_max[2];

	outside_polys[0].points[9] = bbox_min[0];
	outside_polys[0].points[10] = bbox_min[1];
	outside_polys[0].points[11] = bbox_max[2];

	/***********************************************/

	outside_planes[1].n[0] = -1.0;
	outside_planes[1].n[1] = 0.0;
	outside_planes[1].n[2] = 0.0;
	outside_planes[1].d = bbox_max[0];

	outside_polys[1].legal = false;
	outside_polys[1].points.resize(4*3);
	outside_polys[1].points[0] = bbox_max[0];
	outside_polys[1].points[1] = bbox_min[1];
	outside_polys[1].points[2] = bbox_min[2];

	outside_polys[1].points[3] = bbox_max[0];
	outside_polys[1].points[4] = bbox_min[1];
	outside_polys[1].points[5] = bbox_max[2];

	outside_polys[1].points[6] = bbox_max[0];
	outside_polys[1].points[7] = bbox_max[1];
	outside_polys[1].points[8] = bbox_max[2];

	outside_polys[1].points[9] = bbox_max[0];
	outside_polys[1].points[10] = bbox_max[1];
	outside_polys[1].points[11] = bbox_min[2];

	/***********************************************/

	outside_planes[2].n[0] = 0.0;
	outside_planes[2].n[1] = 1.0;
	outside_planes[2].n[2] = 0.0;
	outside_planes[2].d = -bbox_min[1];
	
	outside_polys[2].legal = false;
	outside_polys[2].points.resize(4*3);
	outside_polys[2].points[0] = bbox_min[0];
	outside_polys[2].points[1] = bbox_min[1];
	outside_polys[2].points[2] = bbox_min[2];

	outside_polys[2].points[3] = bbox_min[0];
	outside_polys[2].points[4] = bbox_min[1];
	outside_polys[2].points[5] = bbox_max[2];

	outside_polys[2].points[6] = bbox_max[0];
	outside_polys[2].points[7] = bbox_min[1];
	outside_polys[2].points[8] = bbox_max[2];

	outside_polys[2].points[9] = bbox_max[0];
	outside_polys[2].points[10] = bbox_min[1];
	outside_polys[2].points[11] = bbox_min[2];

	/***********************************************/

	outside_planes[3].n[0] = 0.0;
	outside_planes[3].n[1] = -1.0;
	outside_planes[3].n[2] = 0.0;
	outside_planes[3].d = bbox_max[1];
	
	outside_polys[3].legal = false;
	outside_polys[3].points.resize(4*3);
	outside_polys[3].points[0] = bbox_min[0];
	outside_polys[3].points[1] = bbox_max[1];
	outside_polys[3].points[2] = bbox_min[2];

	outside_polys[3].points[3] = bbox_max[0];
	outside_polys[3].points[4] = bbox_max[1];
	outside_polys[3].points[5] = bbox_min[2];

	outside_polys[3].points[6] = bbox_max[0];
	outside_polys[3].points[7] = bbox_max[1];
	outside_polys[3].points[8] = bbox_max[2];

	outside_polys[3].points[9] = bbox_min[0];
	outside_polys[3].points[10] = bbox_max[1];
	outside_polys[3].points[11] = bbox_max[2];

	/***********************************************/
	
	outside_planes[4].n[0] = 0.0;
	outside_planes[4].n[1] = 0.0;
	outside_planes[4].n[2] = 1.0;
	outside_planes[4].d = -bbox_min[2];
			
	outside_polys[4].legal = false;
	outside_polys[4].points.resize(4*3);
	outside_polys[4].points[0] = bbox_min[0];
	outside_polys[4].points[1] = bbox_min[1];
	outside_polys[4].points[2] = bbox_min[2];

	outside_polys[4].points[3] = bbox_max[0];
	outside_polys[4].points[4] = bbox_min[1];
	outside_polys[4].points[5] = bbox_min[2];

	outside_polys[4].points[6] = bbox_max[0];
	outside_polys[4].points[7] = bbox_max[1];
	outside_polys[4].points[8] = bbox_min[2];

	outside_polys[4].points[9] = bbox_min[0];
	outside_polys[4].points[10] = bbox_max[1];
	outside_polys[4].points[11] = bbox_min[2];

	/***********************************************/

	outside_planes[5].n[0] = 0.0;
	outside_planes[5].n[1] = 0.0;
	outside_planes[5].n[2] = -1.0;
	outside_planes[5].d = bbox_max[2];
				
	outside_polys[5].legal = false;
	outside_polys[5].points.resize(4*3);
	outside_polys[5].points[0] = bbox_min[0];
	outside_polys[5].points[1] = bbox_min[1];
	outside_polys[5].points[2] = bbox_max[2];

	outside_polys[5].points[3] = bbox_min[0];
	outside_polys[5].points[4] = bbox_max[1];
	outside_polys[5].points[5] = bbox_max[2];

	outside_polys[5].points[6] = bbox_max[0];
	outside_polys[5].points[7] = bbox_max[1];
	outside_polys[5].points[8] = bbox_max[2];

	outside_polys[5].points[9] = bbox_max[0];
	outside_polys[5].points[10] = bbox_min[1];
	outside_polys[5].points[11] = bbox_max[2];

	/***********************************************/

	total_polygon_list.reserve(MAX_BRUSH_POLYS * num_brushes);
	total_planes_list.reserve(MAX_BRUSH_POLYS * num_brushes);

	total_polygon_list.insert(total_polygon_list.end(), outside_polys, outside_polys + 6);
	total_planes_list.insert(total_planes_list.end(), outside_planes, outside_planes + 6);

	for(int i = 0; i < num_brushes; i++)
	{
		total_polygon_list.insert(total_polygon_list.end(), brushes[i].polygons().begin(), brushes[i].polygons().end());
		total_planes_list.insert(total_planes_list.end(), brushes[i].planes().begin(), brushes[i].planes().end());
	}

	cout << "total: " << total_planes_list.size() << endl;
	//system("pause");

	total_polygon_list.shrink_to_fit();
	total_planes_list.shrink_to_fit();

	for(unsigned int i = 0; i < total_polygon_list.size(); i++)
	{
		//cout << total_polygon_list[i].points.size()/3 << endl;
		if(total_polygon_list[i].points.size()/3 < 3)
		{
			cout << "awaw" << endl;
			system("pause");
		}

		for(unsigned int j = 0; j < total_polygon_list[i].points.size()/3 - 1; j++)
		{
			if(fabs(total_polygon_list[i].points[j*3] - total_polygon_list[i].points[(j+1)*3]) < epsilon &&
				fabs(total_polygon_list[i].points[j*3+1] - total_polygon_list[i].points[(j+1)*3+1]) < epsilon &&
				fabs(total_polygon_list[i].points[j*3+2] - total_polygon_list[i].points[(j+1)*3+2]) < epsilon)
			{
				cout << "awaw" << endl;
				system("pause");
			}
		}
	}

	srand(SDL_GetTicks());
	for(int i = 0; i < 1024; i++)
	{
		color_table[i][0] = ((float)(rand() % 256))/(256.0f);
		color_table[i][1] = ((float)(rand() % 256))/(256.0f);
		color_table[i][2] = ((float)(rand() % 256))/(256.0f);
	}

	BuildTree(root_node, total_polygon_list, total_planes_list, total_polygon_list);


	cout << "BSPNode::num_nodes = " << BSPNode::num_nodes <<endl;

	unsigned int t1 = SDL_GetTicks();

	vector<Plane> planes_used_as_portals;
	planes_used_as_portals.reserve(128);
	/*
	for(unsigned int i = 0; i < nodes_array.size(); i++)
	{
		if(!nodes_array[i]->has_polys())
		{
			
			nodes_array[i]->GeneratePortals();

		}
	}
	*/

	for(vector<BSPNode*>::iterator node = nodes_array.begin(); node != nodes_array.end(); node++)
	{
		if(!(*node)->has_polys())
		{
			if(find(planes_used_as_portals.begin(), planes_used_as_portals.end(), (*node)->div_plane()) == planes_used_as_portals.end())
			{
				(*node)->GeneratePortals();
				planes_used_as_portals.push_back((*node)->div_plane());
			}
		}
	}
	
	planes_used_as_portals.clear();
	//planes_used_as_portals.resize(0);

	unsigned int t2 = SDL_GetTicks() - t1;

	cout << "portals done in " << t2/1000.0f << " secs" << endl;
	
	for(unsigned int i = 0; i < nodes_array.size(); i++)
	{
		//cout << "ids: " << nodes_array[i]->id() << endl;
		if(nodes_array[i])
		if(nodes_array[i]->HasNonLegal())
		{
			DeleteConnectedNodes(nodes_array, i, g_portals);
			break;
			//i--;
		}
	}
	

	/*
	for(int i = 0; i < nodes_array.size(); i++)
	{
		if(!nodes_array[i])
		{
			delete nodes_array[i];
			nodes_array.erase(nodes_array.begin() + i);
			i--;
		}
	}
	*/

	for(unsigned int i = 0; i < g_portals.size(); i++)
	{
		if(!g_portals[i])
		{
			//delete g_portals[i];
			g_portals.erase(g_portals.begin() + i);
			i--;
		}
	}

	total_polygon_list.clear();
	total_planes_list.clear();

	for(unsigned int i = 0; i < nodes_array.size(); i++)
	{
		if(nodes_array[i])
		{
			if(nodes_array[i]->has_polys())
			{
			vector<Poly> polys2add;
			nodes_array[i]->originals().swap(polys2add);

			for(unsigned int j = 0; j < polys2add.size(); j++)
			{
				bool b = true;
				for(unsigned int k = 0; k < total_polygon_list.size(); k++)
				{
					if(total_polygon_list[k] == polys2add[j])
					{
						b = false;
						break;
					}
				}

				if(b)
				{
					total_polygon_list.push_back(polys2add[j]);
					total_planes_list.push_back(nodes_array[i]->planes()[j]);
				}
			}

			//nodes_array[i]->originals().swap(polys2add);

			//total_polygon_list.insert(total_polygon_list.end(), nodes_array[i]->originals().begin(), nodes_array[i]->originals().end());
			}
		}
	}
	
	cout << "size1 = " << total_polygon_list.size() << endl;
	cout << "size2 = " << total_planes_list.size() << endl;
	//system("pause");

	delete root_node;
	BSPNode::num_nodes = 0;
	g_portals.clear();
	g_prev_dividers.clear();
	nodes_array.clear();
	BuildTree(root_node, total_polygon_list, total_planes_list, total_polygon_list);

	t1 = SDL_GetTicks();

	for(vector<BSPNode*>::iterator node = nodes_array.begin(); node != nodes_array.end(); node++)
	{
		if(!(*node)->has_polys())
		{
			if(find(planes_used_as_portals.begin(), planes_used_as_portals.end(), (*node)->div_plane()) == planes_used_as_portals.end())
			{
				(*node)->GeneratePortals();
				planes_used_as_portals.push_back((*node)->div_plane());
			}
		}
	}

	for(vector<BSPNode*>::iterator node = nodes_array.begin(); node != nodes_array.end(); node++)
	{
		if((*node)->has_polys())
		{
			cout << "num_portals: " << (*node)->leaf_portals().size() << endl;
		}
	}

	planes_used_as_portals.clear();

	t2 = SDL_GetTicks() - t1;

	cout << "portals done in " << t2/1000.0f << " secs" << endl;

	cout << "BSPNode::num_nodes = " << BSPNode::num_nodes <<endl;

	unsigned int t11 = SDL_GetTicks();
/*
	for(vector<BSPNode*>::iterator node = nodes_array.begin(); node != nodes_array.end(); node++)
	{
		if((*node)->has_polys())
		{
			cout << "111" << endl;
			(*node)->BuildPVS();
		}
	}
*/
	/***************************/

	vector<thread> threads;
	//unsigned int shared_index;
	int num_threads = min(thread::hardware_concurrency(), (unsigned int)nodes_array.size());
	auto shrd_iterator = nodes_array.begin();
	std::mutex mtx;
	for(int i = 0;  i < num_threads; i++)
	{
		threads.push_back(thread([&](int id)->void
		{
			auto cur_work = shrd_iterator;
			while(true)
			{
				{
					lock_guard<mutex> lock(mtx);
					cur_work = shrd_iterator;
					if(cur_work != nodes_array.end())
					{
						shrd_iterator++;
					} else{
						return;
					}
				}
				(*cur_work)->BuildPVS();
			}

			/*for(unsigned int i = id; i < nodes_array.size(); i += num_threads)
			{
				if(nodes_array[i]->has_polys())
				{
					nodes_array[i]->BuildPVS();
				}
			}*/
		}, i));
	}

	for(int i = 0;  i < num_threads; i++)
	{
		threads[i].join();
	}

	cout << SDL_GetTicks() - t11 << endl;
	system("pause");

	/***************************/
	for(auto node_it = nodes_array.begin(); node_it != nodes_array.end(); node_it++)
	{
		(*node_it)->UpdateUVs();
	}

	nodes_array.clear();
	root_node->ReorderNodes(nodes_array);
	root_node->UpdateBbox();

	for(auto node_it = nodes_array.begin(); node_it != nodes_array.end(); node_it++)
	{
		(*node_it)->Finish(nodes_array);
	}
	/*
	for(auto node_it = nodes_array.begin(); node_it != nodes_array.end(); node_it++)
	{
		(*node_it)->JoinVisibleFaces();
	}*/

}

int BSPNode::num_nodes = 0;
int BSPNode::num_drawn = 0;

int ChoseDividingPolygon(vector<Poly>& poly_list, vector<Plane>& plane_list, vector<Poly>& prev_dividers) {

	float	min_relation = MINIMUM_RELATION,
			best_relation = 0;

	int	best_polygon = -1,
		best_worst_polygon = -1,
		least_splits = std::numeric_limits<int>::max();//INT_MAX;
	
	int num_polygons = poly_list.size();

	bool convex = true;

	int /*b_num_front = 0, */b_num_back = 0, b_num_spanning = 0;

	bool repeat = false;

	while(best_polygon == -1)
	{
		//cout << endl << endl;
		if(min_relation < 0.8)
		{
		//cout << "NEW*******************************************************" << endl;
		//cout << min_relation << endl;
		}
		
		for(int i = 0; i < num_polygons; i++)
		{
			bool b = true;
			for(unsigned int j = 0; j < prev_dividers.size(); j++)
			{
				if(poly_list[i] == prev_dividers[j])
				{
					b = false;
					break;
				}
			}

			if(!b)
			{
				continue;
			}

			int num_front = 0, num_back = 0, num_spanning = 0;
			

			for(int j = 0; j < num_polygons; j++)
			{
				if(i == j)
				{
					continue;
				}
				float val;
				switch(ClassifyPolygon2(plane_list[i], poly_list[j]))
				{
				case ONPLANE:
					val = dot(plane_list[i].n, plane_list[j].n);
					if(val > 0)
					{
						num_front++;
					} else
					{
						num_back++;
					}
					break;
				case FRONT:
				case TOUCH_FRONT:
					num_front++;
					break;
				case BACK:
				case TOUCH_BACK:
					num_back++;
					break;
				case SPANNING:
					num_spanning++;
					break;
				}
			}

			//cout << "f: " << num_front << "   b: " << num_back << "   s: " << num_spanning <<  "   t: " << num_polygons << endl;

			if((convex) && ((num_back != 0) || (num_spanning != 0)))
			{
				convex = false;
				//system("pause");
			}

			//if(num_front == 0 && num_back == 0)
			//{
			//	cout << "spanning: " << num_spanning << endl;
			//	system("pause");
			//}

			float relation = 0;

			if(num_front < num_back)
			{
				if(num_back != 0)
					relation = (float)num_front / (float)num_back;
			} else
			{
				if(num_front != 0)
					relation = (float)num_back / (float)num_front;
			}
			
			if(relation > 0)
			{
				repeat = true;
			}

			if(((relation > min_relation) && (num_spanning <= least_splits) /*&& ((num_back != 0) || (num_spanning != 0))*/) ||
				((num_spanning <= least_splits) && (relation > best_relation) /*&& ((num_back != 0) || (num_spanning != 0))*/))

			//if(relation >= best_relation)

			//if(((relation > min_relation) && (num_spanning < least_splits)) ||
			//	((num_spanning <= least_splits) && (relation >= best_relation)) ||
			//	(((num_front == 0) && (num_back == 0))&& (best_relation == 0)))
			{
				best_polygon = i;
				//if(relation > 0)
					least_splits = num_spanning;
				best_relation = relation;

				/*b_num_front = num_front, */b_num_back = num_back, b_num_spanning = num_spanning;
			} else
			{
				//cout << "br: " << best_relation << "   ls: " << least_splits << endl;
				
				if(!(relation > 0)  && ((num_back != 0) || (num_spanning != 0)))
				{
					best_worst_polygon = i;
				}

			}
		}

		if(convex)
		{
			//cout << "convex!!" << endl;
			//system("pause");
			return NO_NEED_TO_DIVIDE;
		}

		if(!repeat)
		{
			return best_worst_polygon;
		}

		min_relation /= MIN_RELATION_SCALE;

	}

	//cout << "bf: " << b_num_front << "   bb: " << b_num_back << "   bs: " << b_num_spanning <<  "   bt: " << num_polygons << endl;

	if((b_num_back == 0) && (b_num_spanning == 0))
		system("pause");


	return best_polygon;

}

void BuildTree(BSPNode*& node, vector<Poly>& polygons, vector<Plane>& planes, vector<Poly>& original_polys) {

	int num_polys = polygons.size();
	//cout << "num_polys: " << num_polys << endl;

	if(num_polys == 0)
	{
		node = NULL;
		//system("pause");
		return;
	}

	int best_div_poly = ChoseDividingPolygon(polygons, planes, g_prev_dividers);

	if(best_div_poly == NO_NEED_TO_DIVIDE)
	{
		node = new BSPNode(polygons, planes);
		node->set_originals(original_polys);
		//nodes_array.insert(nodes_array.begin(), node);
		nodes_array.push_back(node);
		return;
	}

	g_prev_dividers.push_back(polygons[best_div_poly]);

	vector<Poly> front_poly_list, back_poly_list;
	vector<Plane> front_plane_list, back_plane_list;

	front_poly_list.push_back(polygons[best_div_poly]);
	front_plane_list.push_back(planes[best_div_poly]);

	vector<Poly> front_originals, back_originals;
	front_originals.push_back(original_polys[best_div_poly]);


	for(int i = 0; i < num_polys; i++)
	{
		if(i == best_div_poly)
		{
			continue;
		}

		float val;
		switch(ClassifyPolygon2(planes[best_div_poly], polygons[i]))
		{
		case ONPLANE:
			val = dot(planes[best_div_poly].n, planes[i].n);
			if(val > 0)
			{
				front_poly_list.push_back(polygons[i]);
				front_plane_list.push_back(planes[i]);

				front_originals.push_back(original_polys[i]);
			} else
			{
				back_poly_list.push_back(polygons[i]);
				back_plane_list.push_back(planes[i]);

				back_originals.push_back(original_polys[i]);
			}
			break;
		case FRONT:
		case TOUCH_FRONT:
			front_poly_list.push_back(polygons[i]);
			front_plane_list.push_back(planes[i]);

			front_originals.push_back(original_polys[i]);
			break;
		case BACK:
		case TOUCH_BACK:
			back_poly_list.push_back(polygons[i]);
			back_plane_list.push_back(planes[i]);

			back_originals.push_back(original_polys[i]);
			break;
		case SPANNING:
			Poly front, back;

			SplitPoly(polygons[i], planes[best_div_poly], front, back);

			front.legal = polygons[i].legal;
			back.legal = polygons[i].legal;
			//if(!polygons[i].legal)
			//{
			//	front.legal = false;
			//	back.legal = false;
			//}
			
			front_poly_list.push_back(front);
			back_poly_list.push_back(back);

			front_plane_list.push_back(planes[i]);
			back_plane_list.push_back(planes[i]);

			front_originals.push_back(original_polys[i]);
			back_originals.push_back(original_polys[i]);

			break;
		}

	}

	BSPNode* front_node; 
	BSPNode* back_node;

	//front_poly_list.swap(g_front_poly_list);
	//back_poly_list.swap(g_back_poly_list);

	if(front_poly_list.empty())
	{
		cout << "omg" << endl;
		system("pause");
	}
	
	if(back_poly_list.empty())
	{
		cout << "omg1" << endl;
		system("pause");
	}

	BuildTree(front_node, front_poly_list, front_plane_list, front_originals);
	//front_node->set_originals(front_originals);
	BuildTree(back_node, back_poly_list, back_plane_list, back_originals);
	//back_node->set_originals(back_originals);

	node = new BSPNode(front_node, back_node, planes[best_div_poly]);
	//front_node->set_parent_node(node);
	//back_node->set_parent_node(node);

	//nodes_array.insert(nodes_array.begin(), node);
	nodes_array.push_back(node);

}


BSPNode::BSPNode(BSPNode* front, BSPNode* back, Plane divider) :	div_plane_(divider),
																	front_node_(front),
																	back_node_(back),
																	parent_node_(NULL),
																	//polygons_(NULL),
																	//planes_(NULL),
																	has_polys_(false){

	bbox_min_[0] = numeric_limits<float>::max();
	bbox_min_[1] = numeric_limits<float>::max();
	bbox_min_[2] = numeric_limits<float>::max();

	bbox_max_[0] = -numeric_limits<float>::max();
	bbox_max_[1] = -numeric_limits<float>::max();
	bbox_max_[2] = -numeric_limits<float>::max();

	front_node_->set_parent_node(this);
	back_node_->set_parent_node(this);

	id_ = num_nodes;
	num_nodes++;

}

BSPNode::BSPNode(vector<Poly>& polygons, vector<Plane>& planes) :	front_node_(NULL),
																	back_node_(NULL),
																	parent_node_(NULL),
																	has_polys_(true){

	polygons_.swap(polygons);
	planes_.swap(planes);

	bbox_min_[0] = numeric_limits<float>::max();
	bbox_min_[1] = numeric_limits<float>::max();
	bbox_min_[2] = numeric_limits<float>::max();

	bbox_max_[0] = -numeric_limits<float>::max();
	bbox_max_[1] = -numeric_limits<float>::max();
	bbox_max_[2] = -numeric_limits<float>::max();

	for(vector<Poly>::iterator p = polygons_.begin(); p != polygons_.end(); p++)
	{
		for(vector<float>::iterator point = p->points.begin(); point != p->points.end(); point += 3)
		{
			bbox_min_[0] = min(bbox_min_[0], point[0]);
			bbox_min_[1] = min(bbox_min_[1], point[1]);
			bbox_min_[2] = min(bbox_min_[2], point[2]);

			bbox_max_[0] = max(bbox_max_[0], point[0]);
			bbox_max_[1] = max(bbox_max_[1], point[1]);
			bbox_max_[2] = max(bbox_max_[2], point[2]);
		}
	}

	center_[0] = (bbox_min_[0] + bbox_max_[0])/2.0f;
	center_[1] = (bbox_min_[1] + bbox_max_[1])/2.0f;
	center_[2] = (bbox_min_[2] + bbox_max_[2])/2.0f;

	bbox_points_[0][0] = bbox_min_[0];
	bbox_points_[0][1] = bbox_min_[1];
	bbox_points_[0][2] = bbox_min_[2];

	bbox_points_[1][0] = bbox_max_[0];
	bbox_points_[1][1] = bbox_min_[1];
	bbox_points_[1][2] = bbox_min_[2];

	bbox_points_[2][0] = bbox_max_[0];
	bbox_points_[2][1] = bbox_min_[1];
	bbox_points_[2][2] = bbox_max_[2];

	bbox_points_[3][0] = bbox_min_[0];
	bbox_points_[3][1] = bbox_min_[1];
	bbox_points_[3][2] = bbox_max_[2];

	bbox_points_[4][0] = bbox_max_[0];
	bbox_points_[4][1] = bbox_max_[1];
	bbox_points_[4][2] = bbox_max_[2];

	bbox_points_[5][0] = bbox_min_[0];
	bbox_points_[5][1] = bbox_max_[1];
	bbox_points_[5][2] = bbox_max_[2];

	bbox_points_[6][0] = bbox_min_[0];
	bbox_points_[6][1] = bbox_max_[1];
	bbox_points_[6][2] = bbox_min_[2];

	bbox_points_[7][0] = bbox_max_[0];
	bbox_points_[7][1] = bbox_max_[1];
	bbox_points_[7][2] = bbox_min_[2];

	id_ = num_nodes;
	num_nodes++;

	//cout << "size: " << polygons_.size() << endl;
	//system("pause");


}

BSPNode::~BSPNode() {
	if(front_node_)
		delete front_node_;
	if(back_node_)
		delete back_node_;
	BSPNode::num_nodes--;
}

void BSPNode::GeneratePortals() {
	
	//cout << "1111: " << endl;
	//system("pause");
	if(has_polys_)
	{
		return;
	}

	float points[3 * 4];

	float up[3] = {0.0, 0.0, 0.0};

	if ((abs(div_plane_.n[0]) <= abs(div_plane_.n[1])) && (abs(div_plane_.n[0]) <= abs(div_plane_.n[2])))
	{
		up[0] = 1.0;
	} else
	{
		if ((abs(div_plane_.n[1]) <= abs(div_plane_.n[0])) && (abs(div_plane_.n[1]) <= abs(div_plane_.n[2])))
		{
			up[1] = 1.0;
		} else
			if ((abs(div_plane_.n[2]) <= abs(div_plane_.n[0])) && (abs(div_plane_.n[2]) <= abs(div_plane_.n[1])))
			{
				up[2] = 1.0;
			}
	}

	float center[3];
	center[0] = -div_plane_.n[0] * div_plane_.d;
	center[1] = -div_plane_.n[1] * div_plane_.d;
	center[2] = -div_plane_.n[2] * div_plane_.d;


	float side[3];
	Cross(side, div_plane_.n, up);

	Cross(up, div_plane_.n, side);
	Normalize(up);

	points[0*3 + 0] = center[0] + side[0]*g_max_dist + up[0]*g_max_dist;
	points[0*3 + 1] = center[1] + side[1]*g_max_dist + up[1]*g_max_dist;
	points[0*3 + 2] = center[2] + side[2]*g_max_dist + up[2]*g_max_dist;

	points[1*3 + 0] = center[0] - side[0]*g_max_dist + up[0]*g_max_dist;
	points[1*3 + 1] = center[1] - side[1]*g_max_dist + up[1]*g_max_dist;
	points[1*3 + 2] = center[2] - side[2]*g_max_dist + up[2]*g_max_dist;

	points[2*3 + 0] = center[0] - side[0]*g_max_dist - up[0]*g_max_dist;
	points[2*3 + 1] = center[1] - side[1]*g_max_dist - up[1]*g_max_dist;
	points[2*3 + 2] = center[2] - side[2]*g_max_dist - up[2]*g_max_dist;

	points[3*3 + 0] = center[0] + side[0]*g_max_dist - up[0]*g_max_dist;
	points[3*3 + 1] = center[1] + side[1]*g_max_dist - up[1]*g_max_dist;
	points[3*3 + 2] = center[2] + side[2]*g_max_dist- up[2]*g_max_dist;

	Poly initial_portal;
	initial_portal.points.insert(initial_portal.points.end(), points, &points[12]);

	//portals_.push_back(initial_portal);

	//cout << portals_.size() << "-----" << endl;

	Portal init_portal(initial_portal, div_plane_);
	portals_.push_back(init_portal);
	


	//root_node->ClipPortals(portals_).swap(portals_);

	vector<Portal> do_again_portals;

	root_node->ClipPortals_LessRecursion(init_portal, do_again_portals).swap(portals_);

	vector<Portal> new_portals;
	vector<Portal> new_do_again_portals;

	while(!do_again_portals.empty())
	{
		//cout << "000   " << do_again_portals.size() << endl;
		
		new_do_again_portals.clear();
		new_do_again_portals.insert(new_do_again_portals.begin(), do_again_portals.begin(), do_again_portals.end());
		do_again_portals.clear();
		
		for(auto port = new_do_again_portals.begin(); port != new_do_again_portals.end(); port++)
		{
			root_node->ClipPortals_LessRecursion(*port, do_again_portals).swap(new_portals);
			portals_.insert(portals_.end(), new_portals.begin(), new_portals.end());
		}


	}


	//front_node_->ClipPortals(portals_).swap(portals_);
	//back_node_->ClipPortals(portals_).swap(portals_);



	for(unsigned int i = 0; i < portals_.size(); i++)
	{
		if(!portals_[i].front_node() || !portals_[i].back_node())
		{
			portals_.erase(portals_.begin() + i);
			i--;
		} else
		{
			bool b= true;
			/*for(unsigned int j = 0; j < portals_[i].polygon().points.size()/3; i++)
			{
				float* p1 = &portals_[i].polygon().points[j*3];
				float* p2;
				if(j == portals_[i].polygon().points.size()/3 - 1)
				{
					p2 = &portals_[i].polygon().points[0];
				} else
				{
					p2 = &portals_[i].polygon().points[(j + 1)*3];
				}
				float edge[3] = {	p2[0] - p1[0],
									p2[1] - p1[1],
									p2[2] - p1[2]};
				if(dot(edge, edge) < epsilon)
				{
					portals_.erase(portals_.begin() + i);
					i--;
					b = false;
					break;
				}
			}*/

			for(vector<Portal*>::iterator portal = g_portals.begin(); portal != g_portals.end(); portal++)
			{
				if((*portal)->polygon() == portals_[i].polygon())
				{
					portals_.erase(portals_.begin() + i);
					i--;
					b = false;
					break;
				}
			}

			if(b)
			{
				g_portals.push_back(&portals_[i]);
				//portals_[i].front_node()->AddPortalIn(&portals_[i]);
				//portals_[i].back_node()->AddPortalOut(&portals_[i]);
				portals_[i].front_node()->AddPortal(&portals_[i]);
				portals_[i].back_node()->AddPortal(&portals_[i]);
			}
		}
	}

	//for(unsigned int i = 0; i < portals_.size(); i++)
	//{
		//cout << "portal: " << portals_[i].front_node() << " " << portals_[i].back_node() << endl;
	//}

	//front_node_->ClipPortals(div_plane_, portals_).swap(new_portals);
	//portals_.swap(new_portals);
	//back_node_->ClipPortals(div_plane_, new_portals).swap(portals_);

	//ClipPolyToList (div_plane_, initial_portal, brush.planes(), 0, clip_on_plane).swap(polys2add);

	//cout << portals_.size() << "-++--" << endl;

	//cout << new_portals[0].points.size() << "-----" << endl;

	//system("pause");

	//front_node_->GeneratePortals();
	//back_node_->GeneratePortals();

}

int num_calls = 0;

vector<Portal> BSPNode::ClipPortals(vector<Portal>& portals) {
	
	//num_calls++;
	//cout << "num_calls = " << num_calls << endl;
	//cout << "1111: " << endl;

	vector<Portal> return_portals;
	return_portals.reserve(16);
	int num_polys = portals.size();

	if(has_polys_)
	{
		//cout << "iiiiiiiii: " << num_polys << endl;
		for(int i = 0; i < num_polys; i++)
		{
			//Portal new_portal();
			vector<Poly> polys2add;
			ClipPolyToList2 (portals[i].plane(), portals[i].polygon(), planes_, polygons_, 0, false).swap(polys2add);
			//cout << "sizesize: " << polys2add.size() << endl;

			for(unsigned int j = 0; j < polys2add.size(); j++)
			{
				Portal new_portal(polys2add[j], portals[i].plane(), portals[i].front_node(), portals[i].back_node());
				//new_portal.set_front_node(portals[i].front_node());
				//new_portal.set_back_node(portals[i].back_node());
				return_portals.push_back(new_portal);
			}

			//return_portals.insert(return_portals.end(), polys2add.begin(), polys2add.end());
		}
		/*
		for(auto portal = return_portals.begin(); portal != return_portals.end(); portal++)
		{
			switch(ClassifyPoint((*portal).plane(), center_))
			{
			case FRONT:
				(*portal).set_front_node(this);
				break;
			case BACK:
				(*portal).set_back_node(this);
				break;
			}
		}*/
		
		for(unsigned int i = 0; i < return_portals.size(); i++)
		{
			
			for(unsigned int j = 0; j < polygons_.size(); j++)
			{
					
				switch(ClassifyPolygon2(return_portals[i].plane(), polygons_[j]))
				{
				case ONPLANE:
					if(j == polygons_.size() - 1)
					{
						if(dot(return_portals[i].plane().n, planes_[j].n) > 0)
						{
							return_portals[i].set_front_node(this);
						} else
						{
							return_portals[i].set_back_node(this);
						}
					}
					break;
				case TOUCH_FRONT:
					if(j == polygons_.size() - 1)
					{
						return_portals[i].set_front_node(this);
						//j = polygons_.size();
					}
					break;
				case FRONT:
					return_portals[i].set_front_node(this);
					j = polygons_.size();
					break;
				case TOUCH_BACK:
					if(j == polygons_.size() - 1)
					{
						return_portals[i].set_back_node(this);
						//j = polygons_.size();
					}
					break;
				case BACK:
					return_portals[i].set_back_node(this);
					j = polygons_.size();
					break;
				}
			}
		}
		
		//cout << "sizesize: " << return_portals.size() << endl;
	} else
	{

		Poly front, back;
		vector<Portal> front_list;
		vector<Portal> back_list;
		vector<Portal> temp_ports1;
		vector<Portal> temp_ports2;

		front_list.reserve(16);
		back_list.reserve(16);
		temp_ports1.reserve(16);
		temp_ports2.reserve(16);


		for(int i = 0; i < num_polys; i++)
		{
			bool b = true;
			for(int k = 0; k < 6; k++)
			{
				int res = ClassifyPolygon2(outside_planes[k], portals[i].polygon());
				if(res == BACK || res == TOUCH_BACK)
				{
					b = false;
					num_calls++;
					//cout << "portal killed!!!!" << endl;
					break;
				}
					
			}

			if(b)
			switch(ClassifyPolygon2 (div_plane_, portals[i].polygon()))
			{
			case SPANNING:
				front_list.clear();
				back_list.clear();
				SplitPoly(portals[i].polygon(), div_plane_, front, back);

				front_list.push_back(Portal(front, portals[i].plane(), portals[i].front_node(), portals[i].back_node()));
				back_list.push_back(Portal(back, portals[i].plane(), portals[i].front_node(), portals[i].back_node()));
				//back_list.push_back(back);

				temp_ports1.clear();
				front_node_->ClipPortals(front_list).swap(temp_ports1);
				//root_node->ClipPortals(front_list).swap(temp_ports1);

				temp_ports2.clear();
				//root_node->ClipPortals(back_list).swap(temp_ports2);
				back_node_->ClipPortals(back_list).swap(temp_ports2);
				return_portals.insert(return_portals.end(), temp_ports1.begin(), temp_ports1.end());
				return_portals.insert(return_portals.end(), temp_ports2.begin(), temp_ports2.end());

				break;
			case ONPLANE:
				front_list.clear();
				front_list.push_back(portals[i]);
				//back_list.push_back(polys[i]);
				temp_ports1.clear();
				front_node_->ClipPortals(front_list).swap(temp_ports1);
				//return_portals.insert(return_portals.end(), temp_polys1.begin(), temp_polys1.end());
				temp_ports2.clear();
				back_node_->ClipPortals(temp_ports1).swap(temp_ports2);
				return_portals.insert(return_portals.end(), temp_ports2.begin(), temp_ports2.end());
				break;
			case FRONT:
			case TOUCH_FRONT:
				front_list.clear();
				front_list.push_back(portals[i]);
				temp_ports1.clear();
				front_node_->ClipPortals(front_list).swap(temp_ports1);
				return_portals.insert(return_portals.end(), temp_ports1.begin(), temp_ports1.end());
				break;
			case BACK:
			case TOUCH_BACK:
				back_list.clear();
				back_list.push_back(portals[i]);
				temp_ports1.clear();
				back_node_->ClipPortals(back_list).swap(temp_ports1);
				return_portals.insert(return_portals.end(), temp_ports1.begin(), temp_ports1.end());
				break;

			}
		}
		/*
		vector<Poly> polys1;
		vector<Poly> polys2;

		front_node_->ClipPortals(div_plane_, front_list).swap(polys1);

		back_node_->ClipPortals(div_plane_, back_list).swap(polys2);

		return_portals.insert(return_portals.end(), polys1.begin(), polys1.end());
		return_portals.insert(return_portals.end(), polys2.begin(), polys2.end());*/

	}
	//cout << "sizesize: " << return_portals[0].points.size() << endl;
	//cout << "num_calls = " << num_calls << endl;
	//num_calls = 0;
	return return_portals;

}


vector<Portal> BSPNode::ClipPortals_LessRecursion(Portal& portal, vector<Portal>& do_again_portals) {
	
	num_calls++;
	//cout << "num_calls = " << num_calls << endl;
	//cout << "1111: " << endl;

	vector<Portal> return_portals;
	return_portals.reserve(16);

	if(has_polys_)
	{

		vector<Poly> polys2add;
		ClipPolyToList2 (portal.plane(), portal.polygon(), planes_, polygons_, 0, false).swap(polys2add);


		for(unsigned int j = 0; j < polys2add.size(); j++)
		{
			Portal new_portal(polys2add[j], portal.plane(), portal.front_node(), portal.back_node());

			return_portals.push_back(new_portal);
		}


		for(unsigned int i = 0; i < return_portals.size(); i++)
		{
			for(unsigned int j = 0; j < polygons_.size(); j++)
			{
				switch(ClassifyPolygon2(return_portals[i].plane(), polygons_[j]))
				{
				case ONPLANE:
					if(j == polygons_.size() - 1)
					{
						if(dot(return_portals[i].plane().n, planes_[j].n) > 0)
						{
							return_portals[i].set_front_node(this);
						} else
						{
							return_portals[i].set_back_node(this);
						}
					}
					break;
				case TOUCH_FRONT:
					if(j == polygons_.size() - 1)
					{
						return_portals[i].set_front_node(this);
						//j = polygons_.size();
					}
					break;
				case FRONT:
					return_portals[i].set_front_node(this);
					j = polygons_.size();
					break;
				case TOUCH_BACK:
					if(j == polygons_.size() - 1)
					{
						return_portals[i].set_back_node(this);
						//j = polygons_.size();
					}
					break;
				case BACK:
					return_portals[i].set_back_node(this);
					j = polygons_.size();
					break;
				//system("pause");
				}
			}
		}
		
		//cout << "sizesize: " << return_portals.size() << endl;
	} else
	{

		Poly front, back;
		vector<Portal> front_list;
		vector<Portal> back_list;
		vector<Portal> spanning_list;
		vector<Portal> onplane_list;

		front_list.reserve(16);
		back_list.reserve(16);
		spanning_list.reserve(16);
		onplane_list.reserve(16);



		bool b = true;
		for(int k = 0; k < 6; k++)
		{
			int res = ClassifyPolygon2(outside_planes[k], portal.polygon());
			if(res == BACK || res == TOUCH_BACK)
			{
				b = false;

				break;
			}
					
		}

		if(b)
		switch(ClassifyPolygon2 (div_plane_, portal.polygon()))
		{
		case SPANNING:
			SplitPoly(portal.polygon(), div_plane_, front, back);

			do_again_portals.push_back(Portal(front, portal.plane(), portal.front_node(), portal.back_node()));
			do_again_portals.push_back(Portal(back, portal.plane(), portal.front_node(), portal.back_node()));
				
			break;
		case ONPLANE:
			front_node_->ClipPortals_LessRecursion(portal, do_again_portals).swap(front_list);

			for(auto port2 = front_list.begin(); port2 != front_list.end(); port2++)
			{
				back_node_->ClipPortals_LessRecursion(*port2, do_again_portals).swap(back_list);
				return_portals.insert(return_portals.end(), back_list.begin(), back_list.end());
			}
			break;
		case FRONT:
		case TOUCH_FRONT:
			front_node_->ClipPortals_LessRecursion(portal, do_again_portals).swap(front_list);
			return_portals.insert(return_portals.end(), front_list.begin(), front_list.end());
			break;
		case BACK:
		case TOUCH_BACK:
			back_node_->ClipPortals_LessRecursion(portal, do_again_portals).swap(back_list);
			return_portals.insert(return_portals.end(), back_list.begin(), back_list.end());
			break;

		}


	}
	//cout << "sizesize: " << return_portals[0].points.size() << endl;
	//cout << "num_calls = " << num_calls << endl;
	//num_calls = 0;
	return return_portals;

}

void BSPNode::GetUsedTextures(std::set<std::shared_ptr<GLTexture>>& list)
{
	for(auto& poly : polygons_)
	{
		list.insert(poly.tex);
	}
}

bool BSPNode::HasNonLegal() {

	if(!has_polys_)
	{
		return false;
	}

	for(unsigned int i = 0; i < polygons_.size(); i++){
		if(!polygons_[i].legal)
		{
			return true;
		}
	}

	return false;
}

void BSPNode::UpdateUVs() {

	float rot_matrix[16];

	for(unsigned int i = 0; i < polygons_.size(); i++)
	{
		polygons_[i].uvs.clear();
		polygons_[i].uvs.reserve(2 * polygons_[i].points.size() / 3);

		const float* normal = planes_[i].n;

		float sum = abs(polygons_[i].tex_vec[0][0]) + abs(polygons_[i].tex_vec[0][1]) + abs(polygons_[i].tex_vec[0][2]);

		float mag_u = 1.0f, mag_v = 1.0f;
		if(sum > epsilon)
		{
			mag_u = sqrt(dot(polygons_[i].tex_vec[0], polygons_[i].tex_vec[0]));
			mag_v = sqrt(dot(polygons_[i].tex_vec[1], polygons_[i].tex_vec[1]));
		}

		//if(sum < epsilon)
		if(abs(normal[0]) >= abs(normal[1]) && abs(normal[0]) >= abs(normal[2]))
		{
			polygons_[i].tex_vec[0][0] = 0.0f;	polygons_[i].tex_vec[0][1] = 0.0f;	polygons_[i].tex_vec[0][2] = 1.0f;
			polygons_[i].tex_vec[1][0] = 0.0f;	polygons_[i].tex_vec[1][1] = 1.0f;	polygons_[i].tex_vec[1][2] = 0.0f;
		} else
		{
			if(abs(normal[1]) >= abs(normal[0]) && abs(normal[1]) >= abs(normal[2]))
			{
				polygons_[i].tex_vec[0][0] = 1.0f;	polygons_[i].tex_vec[0][1] = 0.0f;	polygons_[i].tex_vec[0][2] = 0.0f;
				polygons_[i].tex_vec[1][0] = 0.0f;	polygons_[i].tex_vec[1][1] = 0.0f;	polygons_[i].tex_vec[1][2] = 1.0f;
			} else
			{
				polygons_[i].tex_vec[0][0] = 1.0f;	polygons_[i].tex_vec[0][1] = 0.0f;	polygons_[i].tex_vec[0][2] = 0.0f;
				polygons_[i].tex_vec[1][0] = 0.0f;	polygons_[i].tex_vec[1][1] = 1.0f;	polygons_[i].tex_vec[1][2] = 0.0f;
			}
		}

		if(sum > epsilon)
		{
			polygons_[i].tex_vec[0][0] *= mag_u;
			polygons_[i].tex_vec[0][1] *= mag_u;
			polygons_[i].tex_vec[0][2] *= mag_u;

			polygons_[i].tex_vec[1][0] *= mag_v;
			polygons_[i].tex_vec[1][1] *= mag_v;
			polygons_[i].tex_vec[1][2] *= mag_v;
		}

		float axis[3];
		Cross(axis, &polygons_[i].tex_vec[0][0], &polygons_[i].tex_vec[1][0]);
		Normalize(axis);
		matrixSetIdentityM(rot_matrix);
		matrixSetRotateM(rot_matrix, polygons_[i].tex_angle, axis[0], axis[1], axis[2]);
		float new_vecs[2][3];

		MatVecMult(&polygons_[i].tex_vec[0][0], rot_matrix, &new_vecs[0][0]);
		MatVecMult(&polygons_[i].tex_vec[1][0], rot_matrix, &new_vecs[1][0]);

		copy(&new_vecs[0][0], &new_vecs[0][0] + 3, &polygons_[i].tex_vec[0][0]);
		copy(&new_vecs[1][0], &new_vecs[1][0] + 3, &polygons_[i].tex_vec[1][0]);


		for(auto point_it = polygons_[i].points.begin(); point_it != polygons_[i].points.end(); point_it += 3)
		{
			float u =	point_it[0] *polygons_[i].tex_vec[0][0] + point_it[1] *polygons_[i].tex_vec[0][1] +
						point_it[2] *polygons_[i].tex_vec[0][2] + polygons_[i].tex_vec[0][3];
			float v =	point_it[0] *polygons_[i].tex_vec[1][0] + point_it[1] *polygons_[i].tex_vec[1][1] +
						point_it[2] *polygons_[i].tex_vec[1][2] + polygons_[i].tex_vec[1][3];
			polygons_[i].uvs.push_back(u/512.0f);
			polygons_[i].uvs.push_back(v/512.0f);
		}

	}

}

int BSPNode::Traverse(const float pos[3]) {

	if(has_polys_)
	{
		return id_;
	}

	float res = dot(pos, div_plane_.n) + div_plane_.d;

	if(res > 0)
	{
		return front_node_->Traverse(pos);
	} else
	{
		return back_node_->Traverse(pos);
	}

}

void BSPNode::BuildPVS() {

	if(!has_polys_)
	{
		return;
	}
	int t1 = SDL_GetTicks();
	for(vector<Portal*>::iterator source_portal = leaf_portals_.begin(); source_portal != leaf_portals_.end(); source_portal++)
	{
		BSPNode* target_leaf;
		if((*source_portal)->back_node() != this)
		{
			target_leaf = (*source_portal)->back_node();
		} else
		{
			target_leaf = (*source_portal)->front_node();
		}

		pvs_.push_back(target_leaf);

		for(vector<Portal*>::iterator target_portal = target_leaf->leaf_portals().begin(); target_portal != target_leaf->leaf_portals().end(); target_portal++)
		{
			if(! ((*source_portal)->plane() == (*target_portal)->plane()) && (ClassifyPolygon2((*source_portal)->plane(), (*target_portal)->polygon()) != ONPLANE))
			{

				if(	fabs((*source_portal)->plane().n[0] - (*target_portal)->plane().n[0]) < epsilon &&
					fabs((*source_portal)->plane().n[1] - (*target_portal)->plane().n[1]) < epsilon &&
					fabs((*source_portal)->plane().n[2] - (*target_portal)->plane().n[2]) < epsilon &&
					fabs((*source_portal)->plane().d - (*target_portal)->plane().d) < epsilon)
				{
					continue;
				}

				//vector<BSPNode*> new_pvs;
				vector<Portal*> portal_chain;
				vector<BSPNode*> nodes_chain;
				nodes_chain.push_back(target_leaf);
				portal_chain.push_back(*source_portal);
				portal_chain.push_back(*target_portal);
				//cout << "222" << endl;


				//RecursePVS(this, *source_portal, target_leaf, *target_portal, 1).swap(new_pvs);
				RecursePVS2(this, *source_portal, target_leaf, *target_portal, 1, pvs_, portal_chain, nodes_chain);

				/*for(auto p_node : new_pvs)
				{
					p_node->AddToPvs(this);
				}*/

				//pvs_.insert(pvs_.end(), new_pvs.begin(), new_pvs.end());
			}
		}
	}
	cout << "time = " << SDL_GetTicks() - t1 << endl;

	sort(pvs_.begin(), pvs_.end());
	pvs_.erase(unique(pvs_.begin(), pvs_.end()), pvs_.end());
}

int num = 0;

vector<BSPNode*> RecursePVS(BSPNode* source_leaf, Portal* source_portal, BSPNode* target_leaf, Portal* target_portal, int depth) {
	static int recursion = 0;
	recursion++;
	vector<BSPNode*> ret_val;

	BSPNode* generator_leaf;
	if(target_leaf != target_portal->front_node())
	{
		generator_leaf = target_portal->front_node();
	} else
	{
		generator_leaf = target_portal->back_node();
	}


	ret_val.push_back(generator_leaf);

	if(depth > 100)
	{
		return ret_val;
	}

	//int source_leaf_location = ClassifyPoint(source_portal->plane(), source_leaf->center());
	//int target_leaf_location = ClassifyPoint(target_portal->plane(), target_leaf->center());
	vector<Plane> clip_planes;
	ConstructClipPlanes(source_portal, target_portal, clip_planes);

	for(vector<Portal*>::iterator generator_portal = generator_leaf->leaf_portals().begin(); generator_portal != generator_leaf->leaf_portals().end(); generator_portal++)
	{
		if((*generator_portal) != target_portal)
		{
			int gen_portal_location = ClassifyPolygon2(source_portal->plane(), (*generator_portal)->polygon());

			if(gen_portal_location == TOUCH_FRONT) gen_portal_location = FRONT;
			if(gen_portal_location == TOUCH_BACK) gen_portal_location = BACK;
			int src_leaf_location;
			if(source_portal->front_node() == source_leaf)
			{
				src_leaf_location = FRONT;
			} else
			{
				src_leaf_location = BACK;
			}

			if((gen_portal_location != ONPLANE) && (gen_portal_location != src_leaf_location))
			{
				gen_portal_location = ClassifyPolygon2(target_portal->plane(), (*generator_portal)->polygon());
				if(gen_portal_location == TOUCH_FRONT) gen_portal_location = FRONT;
				if(gen_portal_location == TOUCH_BACK) gen_portal_location = BACK;
				int target_leaf_location;
				if(target_portal->front_node() == target_leaf)
				{
					target_leaf_location = FRONT;
				} else
				{
					target_leaf_location = BACK;
				}

				if((gen_portal_location != ONPLANE) && (gen_portal_location != target_leaf_location))
				{
					//Portal* clipped_gen_portal = new Portal(**generator_portal);
					Portal clipped_gen_portal(**generator_portal);
					//ClipToAntiPenumbra(source_portal, target_portal, clipped_gen_portal);

					//ClipToAntiPenumbra(source_portal, clip_planes, clipped_gen_portal);

					if(!ClipToAntiPenumbra(source_portal, clip_planes, clipped_gen_portal))
					{
						//cout << "**********************" << endl;
						continue;
					}
					//Portal* clipped_src_portal = new Portal(*source_portal);
					Portal clipped_src_portal(*source_portal);
					//ClipToAntiPenumbra(clipped_gen_portal, target_portal, clipped_src_portal);
					if(!ClipToAntiPenumbra(&clipped_gen_portal, target_portal, clipped_src_portal))
					{
						//cout << "**********************" << endl;
						//delete clipped_gen_portal;
						continue;
					}


					vector<BSPNode*> new_val;

					//cout << recursion << endl;
					RecursePVS(source_leaf, &clipped_src_portal, generator_leaf, &clipped_gen_portal, depth + 1).swap(new_val);
					
					//delete clipped_gen_portal;
					//delete clipped_src_portal;

					//for(auto p_node : new_val)
					//{
						//p_node->AddToPvs(source_leaf);
						//source_leaf->AddToPvs(p_node);
					//}

					//cout << "333" << endl;
					ret_val.insert(ret_val.end(), new_val.begin(), new_val.end());
				}
			}
		}
	}

	recursion--;
	return ret_val;
}

void RecursePVS2(BSPNode* source_leaf, Portal* source_portal, BSPNode* target_leaf, Portal* target_portal, int depth, vector<BSPNode*>& out_nodes, vector<Portal*>& portal_chain, vector<BSPNode*>& nodes_chain) {


	BSPNode* generator_leaf;
	if(target_leaf != target_portal->front_node())
	{
		generator_leaf = target_portal->front_node();
	} else
	{
		generator_leaf = target_portal->back_node();
	}

	if(find(nodes_chain.begin(), nodes_chain.end(), generator_leaf) != nodes_chain.end())
	{
		//cout << "skip skip" << endl;
		//return;
	}
	//cout << "no skip" << endl;
	nodes_chain.push_back(generator_leaf);

	//if(find(out_nodes.begin(), out_nodes.end(), generator_leaf) != out_nodes.end())
	//{
	//	return;
	//}

	out_nodes.push_back(generator_leaf);
	//out_nodes.push_back(target_leaf);

	//cout << "depth = " << depth << endl;
	if(depth > 64)
	{
		//cout << "depth = " << depth << endl;
		//Sleep(20);
		//portal_chain.pop_back();

		nodes_chain.pop_back();
		return;
	}


	//int source_leaf_location = ClassifyPoint(source_portal->plane(), source_leaf->center());
	//int target_leaf_location = ClassifyPoint(target_portal->plane(), target_leaf->center());
	vector<Plane> clip_planes;
	ConstructClipPlanes(source_portal, target_portal, clip_planes);

	if(clip_planes.size() == 0)
	{
		cout << "zeeee" << endl;

		//nodes_chain.pop_back();
		//return;
	}


	for(vector<Portal*>::iterator generator_portal = generator_leaf->leaf_portals().begin(); generator_portal != generator_leaf->leaf_portals().end(); generator_portal++)
	{

		if(find(portal_chain.begin(), portal_chain.end(), *generator_portal) != portal_chain.end())
		{
			continue;
		}

		if((*generator_portal) != target_portal)
		{


			int gen_portal_location = ClassifyPolygon2(source_portal->plane(), (*generator_portal)->polygon());

			if(gen_portal_location == SPANNING)
			{
				//gen_portal_location = FRONT;
				//continue;
			}

			if(gen_portal_location == TOUCH_FRONT) gen_portal_location = FRONT;
			if(gen_portal_location == TOUCH_BACK) gen_portal_location = BACK;
			int src_leaf_location;
			if(source_portal->front_node() == source_leaf)
			{
				src_leaf_location = FRONT;
			} else
			{
				src_leaf_location = BACK;
			}

			if((gen_portal_location != ONPLANE) && (gen_portal_location != src_leaf_location))
			{
				gen_portal_location = ClassifyPolygon2(target_portal->plane(), (*generator_portal)->polygon());

				if(gen_portal_location == SPANNING)
				{
					//gen_portal_location = FRONT;
					//continue;
				}

				if(gen_portal_location == TOUCH_FRONT) gen_portal_location = FRONT;
				if(gen_portal_location == TOUCH_BACK) gen_portal_location = BACK;
				int target_leaf_location;
				if(target_portal->front_node() == target_leaf)
				{
					target_leaf_location = FRONT;
				} else
				{
					target_leaf_location = BACK;
				}

				if((gen_portal_location != ONPLANE) && (gen_portal_location != target_leaf_location))
				{

					//Portal* clipped_gen_portal = new Portal(**generator_portal);
					Portal clipped_gen_portal(**generator_portal);
					//ClipToAntiPenumbra(source_portal, target_portal, clipped_gen_portal);

					//ClipToAntiPenumbra(source_portal, clip_planes, clipped_gen_portal);

					if(clip_planes.size() != 0)
					if(!ClipToAntiPenumbra(source_portal, clip_planes, clipped_gen_portal))
					{
						//cout << "**********************" << endl;
						continue;
					}
					//else
						//continue;

					clipped_gen_portal.polygon().MergePointsByPos();
					if(clipped_gen_portal.polygon().points.size() < 9)
					{
						//continue;
					}

					//Portal* clipped_src_portal = new Portal(*source_portal);
					Portal clipped_src_portal(*source_portal);

					vector<Plane> clip_planes2;
					ConstructClipPlanes(&clipped_gen_portal, target_portal, clip_planes2);

					if(clip_planes2.size() == 0)
					{
						//cout << "qwqwqw" << endl;
						//continue;
					} else

					//ClipToAntiPenumbra(clipped_gen_portal, target_portal, clipped_src_portal);
					//if(!ClipToAntiPenumbra(&clipped_gen_portal, target_portal, clipped_src_portal))
					if(!ClipToAntiPenumbra(&clipped_gen_portal, clip_planes2, clipped_src_portal))
					{
						//cout << "**********************" << endl;
						//delete clipped_gen_portal;
						continue;
					}

					clipped_src_portal.polygon().MergePointsByPos();
					if(clipped_src_portal.polygon().points.size() < 9)
					{
						//continue;
					}
/*
					if(clipped_src_portal.polygon().points.size() > 20)
					{
						cout << "size1 = " << clipped_src_portal.polygon().points.size() << endl;
						clipped_gen_portal.polygon().MergePointsByPos();
						clipped_src_portal.polygon().MergePointsByPos();
						cout << "size2 = " << clipped_src_portal.polygon().points.size() << endl;
					}
*/
					//cout << "portal = " << *generator_portal << endl;

					portal_chain.push_back(*generator_portal);

					RecursePVS2(source_leaf, &clipped_src_portal, generator_leaf, &clipped_gen_portal, depth + 1, out_nodes, portal_chain, nodes_chain);

					portal_chain.pop_back();
					//out_nodes.insert(out_nodes.end(), new_val.begin(), new_val.end());

				}
			}
		}
	}

	nodes_chain.pop_back();

}

void BSPNode::AddMesh(GLStaticMesh* mesh){
	if(has_polys_)
	{
		p_static_meshes_.push_back(mesh);
		return;
	} else
	{
		int res[8];
		res[0] = ClassifyPoint(div_plane_, glm::value_ptr(mesh->bbox_min()));
		res[1] = ClassifyPoint(div_plane_, glm::value_ptr(mesh->bbox_max()));


		if(res[0] != res[1] && res[0] != ONPLANE && res[1] != ONPLANE)
		{
			front_node_->AddMesh(mesh);
			back_node_->AddMesh(mesh);
			return;
		}

		int val = res[0];
		if(val == ONPLANE)
		{
			val = res[1];
		}

		res[2] = ClassifyPoint(div_plane_, &glm::vec3(mesh->bbox_min()[0], mesh->bbox_min()[1], mesh->bbox_max()[2])[0]);
		res[3] = ClassifyPoint(div_plane_, &glm::vec3(mesh->bbox_min()[0], mesh->bbox_max()[1], mesh->bbox_min()[2])[0]);
		res[4] = ClassifyPoint(div_plane_, &glm::vec3(mesh->bbox_max()[0], mesh->bbox_min()[1], mesh->bbox_min()[2])[0]);

		res[5] = ClassifyPoint(div_plane_, &glm::vec3(mesh->bbox_min()[0], mesh->bbox_max()[1], mesh->bbox_max()[2])[0]);
		res[6] = ClassifyPoint(div_plane_, &glm::vec3(mesh->bbox_max()[0], mesh->bbox_min()[1], mesh->bbox_max()[2])[0]);
		res[7] = ClassifyPoint(div_plane_, &glm::vec3(mesh->bbox_max()[0], mesh->bbox_max()[1], mesh->bbox_min()[2])[0]);

		if(val == ONPLANE)
		for(int i = 2; i < 8; i++)
		{
			if(res[i] != ONPLANE)
			{
				val = res[i];
				break;
			}
		}

		for(int i = 2; i < 8; i++)
		{
			if(res[i] != val && res[i] != ONPLANE)
			{
				front_node_->AddMesh(mesh);
				back_node_->AddMesh(mesh);
				return;
			}
		}

		if(val == FRONT)
		{
			front_node_->AddMesh(mesh);
		} else
		{
			back_node_->AddMesh(mesh);
		}
	}

}

void BSPNode::ListMeshes(std::set<GLStaticMesh*>& list){
	list.insert(p_static_meshes_.begin(), p_static_meshes_.end());
	for(auto p_node : pvs_)
	{
		list.insert(p_node->p_static_meshes().begin(), p_node->p_static_meshes().end());
	}
}

void ConstructClipPlanes(Portal* source_portal, Portal* target_portal, vector<Plane>& out_planes){

	float edge1[3], edge2[3];
	int portal_location;
	Portal* temp_source, *temp_target;

	if(source_portal->plane() == target_portal->plane())
	{
		return;
	}

	int num_src_points = source_portal->polygon().points.size()/3,
		num_trg_points = target_portal->polygon().points.size()/3;
	out_planes.reserve(num_src_points * num_trg_points * 2);

	vector<float>::iterator next_point;

	for(int loop = 0; loop < 2; loop++)
	{
		if(loop == 0)
		{
			temp_source = source_portal;
			temp_target = target_portal;
		} else
		{
			temp_source = target_portal;
			temp_target = source_portal;
		}
		for(vector<float>::iterator src_point = temp_source->polygon().points.begin();
									 src_point != temp_source->polygon().points.end(); 
									 src_point += 3)
		{
			portal_location = ClassifyPoint(temp_target->plane(), &(*src_point));
			if(portal_location == ONPLANE)
			{
				continue;
			}
			for(vector<float>::iterator trg_point = temp_target->polygon().points.begin();
										 trg_point != temp_target->polygon().points.end(); 
										 trg_point += 3)
			{
				//if((*trg_point) == temp_target->polygon().points.back() - 2)
				if(trg_point == temp_target->polygon().points.end() - 3)
				{
					next_point = temp_target->polygon().points.begin();
				} else
				{
					next_point = trg_point + 3;
				}

				edge1[0] = src_point[0] - trg_point[0];
				edge1[1] = src_point[1] - trg_point[1];
				edge1[2] = src_point[2] - trg_point[2];

				if(fabs(edge1[0]) + fabs(edge1[1]) + fabs(edge1[2]) < epsilon)
				{
					//continue;
				}

				edge2[0] = next_point[0] - trg_point[0];
				edge2[1] = next_point[1] - trg_point[1];
				edge2[2] = next_point[2] - trg_point[2];

				if(fabs(edge2[0]) + fabs(edge2[1]) + fabs(edge2[2]) < epsilon)
				{
					//continue;
				}

				Plane new_plane;

				Cross(new_plane.n, edge1, edge2);

				float l = sqrtf(new_plane.n[0] * new_plane.n[0] + new_plane.n[1]*new_plane.n[1] + new_plane.n[2]*new_plane.n[2]);

				if(l < epsilon)
				{
					cout << "l skip" << endl;
					//system("pause");
					continue;
				}

				float inv_l = 1.0f/l;

				new_plane.n[0] *= inv_l;
				new_plane.n[1] *= inv_l;
				new_plane.n[2] *= inv_l;

				//Normalize(new_plane.n);

				new_plane.d = -dot(new_plane.n, &src_point[0]);

				int src_loc = ClassifyPolygon2(new_plane, temp_source->polygon());
				int trg_loc = ClassifyPolygon2(new_plane, temp_target->polygon());

				if(src_loc == FRONT)
					src_loc = TOUCH_FRONT;
				else
					if(src_loc == BACK)
						src_loc = TOUCH_BACK;

				if(trg_loc == FRONT)
					trg_loc = TOUCH_FRONT;
				else
					if(trg_loc == BACK)
						trg_loc = TOUCH_BACK;

				if(src_loc == ONPLANE)
				{
					if(dot(new_plane.n, temp_source->plane().n) > 0)
					{
						src_loc = TOUCH_FRONT;
					} else
					{
						src_loc = TOUCH_BACK;
					}
				}

				if(trg_loc == ONPLANE)
				{
					if(dot(new_plane.n, temp_target->plane().n) > 0)
					{
						trg_loc = TOUCH_FRONT;
					} else
					{
						trg_loc = TOUCH_BACK;
					}
				}

				if((src_loc == ONPLANE || src_loc == TOUCH_FRONT) && trg_loc == TOUCH_BACK)
				//if(src_loc != TOUCH_BACK && trg_loc != TOUCH_FRONT)
				{
					//if(find(out_planes.begin(), out_planes.end(), new_plane) == out_planes.end())
					//{
						out_planes.push_back(new_plane);
					//}
				}else
				{
					if(src_loc == TOUCH_BACK && (trg_loc == TOUCH_FRONT || trg_loc == ONPLANE))
					//if(src_loc != TOUCH_FRONT && trg_loc != TOUCH_BACK)
					{
						//if(find(out_planes.begin(), out_planes.end(), new_plane) == out_planes.end())
						//{
							out_planes.push_back(new_plane);
						//}
					}
				}


				/*bool bb = true;
				if(src_loc == FRONT || src_loc == TOUCH_FRONT)
				//if(src_loc != BACK && src_loc != TOUCH_BACK )
				{
					if(trg_loc == BACK || trg_loc == TOUCH_BACK)
					{
						bb = false;
						if(find(out_planes.begin(), out_planes.end(), new_plane) == out_planes.end())
						{
							out_planes.push_back(new_plane);
						}
					}
				} //else
				if(bb){
					if(src_loc == BACK || src_loc == TOUCH_BACK)
					//if(src_loc != FRONT && src_loc != TOUCH_FRONT )
					{
						if(trg_loc == FRONT || trg_loc == TOUCH_FRONT)
						{
							if(find(out_planes.begin(), out_planes.end(), new_plane) == out_planes.end())
							{
								out_planes.push_back(new_plane);
							}
						}
					}
				}*/
			}
		}

	}

	if(out_planes.size() == 0)
	{
		//system("pause");
	}
}

bool ClipToAntiPenumbra(Portal* source_portal, vector<Plane>& clip_planes, Portal& clipped_gen_portal) {

	int portal_location;

	for(vector<Plane>::iterator clip_plane = clip_planes.begin();
								 clip_plane != clip_planes.end();
								 clip_plane++)
	{

		portal_location = ClassifyPolygon2((*clip_plane), clipped_gen_portal.polygon());
		int src_portal_location = ClassifyPolygon2((*clip_plane), source_portal->polygon());

		if(src_portal_location == SPANNING)
		{
			float center[3];
			source_portal->polygon().center(center);
			src_portal_location = ClassifyPoint((*clip_plane), center);
			//continue;
		}

		if(portal_location == TOUCH_FRONT)
			portal_location = FRONT;
		else
			if(portal_location == TOUCH_BACK)
				portal_location = BACK;

		if(src_portal_location == TOUCH_FRONT)
			src_portal_location = FRONT;
		else
			if(src_portal_location == TOUCH_BACK)
				src_portal_location = BACK;

		if(src_portal_location == ONPLANE)
		{
			if(dot((*clip_plane).n, source_portal->plane().n) > 0)
			{
				src_portal_location = FRONT;
			} else
			{
				src_portal_location = BACK;
			}
		}

		if((portal_location == src_portal_location) || (portal_location == ONPLANE))
		{
			return false;
		}

		if((portal_location == BACK && src_portal_location == FRONT) || (portal_location == FRONT && src_portal_location == BACK))
		{
			//*clipped_gen_portal = *generator_portal;
			continue;
		}

		if(portal_location == SPANNING)
		{
			Poly front, back;

			SplitPoly(clipped_gen_portal.polygon(), *clip_plane, front, back);

			if(src_portal_location == FRONT)
			{
				Portal new_gen_portal(back, clipped_gen_portal.plane());
				new_gen_portal.set_front_node(clipped_gen_portal.front_node());
				new_gen_portal.set_back_node(clipped_gen_portal.back_node());
				clipped_gen_portal = new_gen_portal;
			} else
			{
				if(src_portal_location == BACK)
				{
					Portal new_gen_portal(front, clipped_gen_portal.plane());
					new_gen_portal.set_front_node(clipped_gen_portal.front_node());
					new_gen_portal.set_back_node(clipped_gen_portal.back_node());
					clipped_gen_portal = new_gen_portal;
				}
			}

		}

	}
	return true;

	//cout << "==  " << clipped_gen_portal->polygon().points.size() << endl;

}

bool ClipToAntiPenumbra(Portal* source_portal, Portal* target_portal, Portal& clipped_gen_portal) {

	float edge1[3], edge2[3]/*, normal[3]*/;

	int portal_location;

	Portal* temp_source, *temp_target;

	int num_src_points = source_portal->polygon().points.size()/3,
		num_trg_points = target_portal->polygon().points.size()/3;

	vector<Plane> clip_planes;
	clip_planes.reserve(num_src_points * num_trg_points * 2);

	vector<float>::iterator next_point;

	for(int loop = 0; loop < 2; loop++)
	{
		if(loop == 0)
		{
			temp_source = source_portal;
			temp_target = target_portal;
		} else
		{
			temp_source = target_portal;
			temp_target = source_portal;
		}
		for(vector<float>::iterator src_point = temp_source->polygon().points.begin();
									 src_point != temp_source->polygon().points.end();
									 src_point += 3)
		{
			portal_location = ClassifyPoint(temp_target->plane(), &(*src_point));
			if(portal_location == ONPLANE)
			{
				continue;
			}
			for(vector<float>::iterator trg_point = temp_target->polygon().points.begin();
										 trg_point != temp_target->polygon().points.end();
										 trg_point += 3)
			{
				//if((*trg_point) == temp_target->polygon().points.back() - 2)
				if(trg_point == temp_target->polygon().points.end() - 3)
				{
					next_point = temp_target->polygon().points.begin();
				} else
				{
					next_point = trg_point + 3;
				}

				edge1[0] = src_point[0] - trg_point[0];
				edge1[1] = src_point[1] - trg_point[1];
				edge1[2] = src_point[2] - trg_point[2];

				edge2[0] = next_point[0] - trg_point[0];
				edge2[1] = next_point[1] - trg_point[1];
				edge2[2] = next_point[2] - trg_point[2];

				Plane new_plane;

				Cross(new_plane.n, edge1, edge2);
				Normalize(new_plane.n);

				new_plane.d = -dot(new_plane.n, &src_point[0]);

				int src_loc = ClassifyPolygon2(new_plane, temp_source->polygon());
				int trg_loc = ClassifyPolygon2(new_plane, temp_target->polygon());

				bool bb = true;
				//if(src_loc == FRONT || src_loc == TOUCH_FRONT)
				if(src_loc != TOUCH_BACK && trg_loc != TOUCH_FRONT)
				{
					//if(trg_loc == BACK || trg_loc == TOUCH_BACK)
					//{
						bb = false;
						if(find(clip_planes.begin(), clip_planes.end(), new_plane) == clip_planes.end())
						{
							clip_planes.push_back(new_plane);
						}
					//}
				} //else
				if(bb){
					//if(src_loc == BACK || src_loc == TOUCH_BACK)
					if(src_loc != TOUCH_FRONT && trg_loc != TOUCH_BACK)
					{
						//if(trg_loc == FRONT || trg_loc == TOUCH_FRONT)
						//{
							if(find(clip_planes.begin(), clip_planes.end(), new_plane) == clip_planes.end())
							{
								clip_planes.push_back(new_plane);
							}
						//}
					}
				}
			}
		}

	}

	for(vector<Plane>::iterator clip_plane = clip_planes.begin();
									 clip_plane != clip_planes.end();
									 clip_plane++)
	{
		portal_location = ClassifyPolygon2((*clip_plane), clipped_gen_portal.polygon());
		int src_portal_location = ClassifyPolygon2((*clip_plane), source_portal->polygon());

		if((portal_location == src_portal_location) || (portal_location == ONPLANE))
		{
			//delete clipped_gen_portal;
			//clipped_gen_portal = NULL;
			return false;
		}
	}

	for(vector<Plane>::iterator clip_plane = clip_planes.begin();
								 clip_plane != clip_planes.end();
								 clip_plane++)
	{
		
		portal_location = ClassifyPolygon2((*clip_plane), clipped_gen_portal.polygon());
		int src_portal_location = ClassifyPolygon2((*clip_plane), source_portal->polygon());

		if(portal_location == TOUCH_FRONT)
			portal_location = FRONT;
		else
			if(portal_location == TOUCH_BACK)
				portal_location = BACK;

		if(src_portal_location == TOUCH_FRONT)
			src_portal_location = FRONT;
		else
			if(src_portal_location == TOUCH_BACK)
				src_portal_location = BACK;

		if((portal_location == src_portal_location) || (portal_location == ONPLANE))
		{
			//delete clipped_gen_portal;
			//clipped_gen_portal = NULL;
			return false;
		}

		if((portal_location == BACK && src_portal_location == FRONT) || (portal_location == FRONT && src_portal_location == BACK))
		{
			//*clipped_gen_portal = *generator_portal;
			continue;
		}

		if(portal_location == SPANNING)
		{
			Poly front, back;

			SplitPoly(clipped_gen_portal.polygon(), *clip_plane, front, back);

			if(src_portal_location == FRONT)
			{
				//Portal* new_gen_portal = new Portal(back, clipped_gen_portal->plane());
				Portal new_gen_portal(back, clipped_gen_portal.plane());
				new_gen_portal.set_front_node(clipped_gen_portal.front_node());
				new_gen_portal.set_back_node(clipped_gen_portal.back_node());
				//cout << "--  " << new_gen_portal->polygon().points.size() << endl;
				//delete clipped_gen_portal;
				clipped_gen_portal = new_gen_portal;
				//cout << "--  " << clipped_gen_portal->polygon().points.size() << endl;
				//return;
			} else
			{
				if(src_portal_location == BACK)
				{
					//Portal* new_gen_portal = new Portal(front, clipped_gen_portal->plane());
					Portal new_gen_portal(front, clipped_gen_portal.plane());
					new_gen_portal.set_front_node(clipped_gen_portal.front_node());
					new_gen_portal.set_back_node(clipped_gen_portal.back_node());
					//cout << "++  " << new_gen_portal->polygon().points.size() << endl;
					//delete clipped_gen_portal;
					clipped_gen_portal = new_gen_portal;
					//cout << "++  " << clipped_gen_portal->polygon().points.size() << endl;
					//return;
				}
			}

		}

	}

	//cout << "==  " << clipped_gen_portal->polygon().points.size() << endl;
	return true;
}


void BSPNode::Draw() {

	if(Renderer::render_state->wireframe_mode)
	{
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	} else 
	{
		glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
	}

	/*for(auto p_mesh : p_static_meshes_)
	{
		p_mesh->Draw();
	}*/

	if(!has_polys_)
	{
		for(unsigned int i = 0; i < portals_.size(); i++) 
		{
			/*glUniform3f(g_render_state.current_program->uniform(COL_UNIFORM), 1.0, 0.0, 0.0);
		
			glEnableVertexAttribArray(g_render_state.current_program->attribute(POSITION_ATTRIBUTE));
			glVertexAttribPointer(g_render_state.current_program->attribute(POSITION_ATTRIBUTE), 3, GL_FLOAT, GL_FALSE, 0, &portals_[i].points[0]);
			//cout << portals_.size() << endl;
			glDrawArrays(GL_POLYGON, 0, portals_[i].points.size() / 3);*/

			portals_[i].Draw();
			
		}

		
		if(front_node_)
			front_node_->Draw();
		if(back_node_)
			back_node_->Draw();

		return;
	}

	
	if(!Renderer::render_state->current_cam->IsInFrustum(bbox_points_))
	{
		return;
	}

	if(polygons_.empty())
	{
		cout << "empty!!!" << endl;
		//system("pause");
		return;
	}
	
	for(unsigned int i = 0; i < polygons_.size(); i++) 
	{
		//glUniform3f(Renderer::render_state->current_program->uniform(COL_UNIFORM), color_table[id_][0], color_table[id_][1], color_table[id_][2]);
		glUniform3f(Renderer::render_state->current_program->uniform(COL_UNIFORM), color_table[i][0], color_table[i][1], color_table[i][2]);
/*
		glEnableVertexAttribArray(Renderer::render_state->current_program->attribute(POSITION_ATTRIBUTE));
		glVertexAttribPointer(Renderer::render_state->current_program->attribute(POSITION_ATTRIBUTE), 3, GL_FLOAT, GL_FALSE, 0, &polygons_[i].points[0]);

		glDrawArrays(GL_TRIANGLE_FAN, 0, polygons_[i].points.size() / 3);*/
		
		Renderer::SwitchTexture(DIFFUSEMAP_SLOT, *polygons_[i].tex);
		/*
		glEnableVertexAttribArray(Renderer::render_state->current_program->attribute(POSITION_ATTRIBUTE));
		glVertexAttribPointer(Renderer::render_state->current_program->attribute(POSITION_ATTRIBUTE), 3, GL_FLOAT, GL_FALSE, 0, &polygons_[i].points[0]);

		glEnableVertexAttribArray(Renderer::render_state->current_program->attribute(UV_ATTRIBUTE));
		glVertexAttribPointer(Renderer::render_state->current_program->attribute(UV_ATTRIBUTE), 2, GL_FLOAT, GL_FALSE, 0, &polygons_[i].uvs[0]);

		glEnableVertexAttribArray(Renderer::render_state->current_program->attribute(NORMAL_ATTRIBUTE));
		glVertexAttribPointer(Renderer::render_state->current_program->attribute(NORMAL_ATTRIBUTE), 3, GL_FLOAT, GL_FALSE, 0, &polygons_[i].normals[0]);

		glDrawArrays(GL_TRIANGLE_FAN, 0, polygons_[i].points.size() / 3);
		//glDrawArrays(GL_POLYGON, 0, polygons_[i].points.size() / 3);

		glDisableVertexAttribArray(Renderer::render_state->current_program->attribute(NORMAL_ATTRIBUTE));
		glDisableVertexAttribArray(Renderer::render_state->current_program->attribute(UV_ATTRIBUTE));
		glDisableVertexAttribArray(Renderer::render_state->current_program->attribute(POSITION_ATTRIBUTE));
		*/

		glEnableVertexAttribArray(Renderer::render_state->current_program->attribute(POSITION_ATTRIBUTE));
		glVertexAttribPointer(Renderer::render_state->current_program->attribute(POSITION_ATTRIBUTE), 3, GL_FLOAT, GL_FALSE, 0, &vertices_pos_list[0]);

		glEnableVertexAttribArray(Renderer::render_state->current_program->attribute(UV_ATTRIBUTE));
		glVertexAttribPointer(Renderer::render_state->current_program->attribute(UV_ATTRIBUTE), 2, GL_FLOAT, GL_FALSE, 0, &vertices_uv_list[0]);

		glEnableVertexAttribArray(Renderer::render_state->current_program->attribute(NORMAL_ATTRIBUTE));
		glVertexAttribPointer(Renderer::render_state->current_program->attribute(NORMAL_ATTRIBUTE), 3, GL_FLOAT, GL_FALSE, 0, &vertices_normal_list[0]);

		//glDrawElements(GL_TRIANGLE_FAN, polygons_[i].vtx_indices.size(), GL_UNSIGNED_SHORT, &polygons_[i].vtx_indices[0]);
		//glDrawElements(GL_TRIANGLE_STRIP, polygons_[i].vtx_indices.size(), GL_UNSIGNED_SHORT, &polygons_[i].vtx_indices[0]);
		glDrawElements(GL_TRIANGLES, polygons_[i].vtx_indices.size(), GL_UNSIGNED_SHORT, &polygons_[i].vtx_indices[0]);

		glDisableVertexAttribArray(Renderer::render_state->current_program->attribute(NORMAL_ATTRIBUTE));
		glDisableVertexAttribArray(Renderer::render_state->current_program->attribute(UV_ATTRIBUTE));
		glDisableVertexAttribArray(Renderer::render_state->current_program->attribute(POSITION_ATTRIBUTE));


	}

	BSPNode::num_drawn++;
	drawn_ = true;
}

void BSPNode::DrawPortals() {
	
	if(drawn_)
	{
		return;
	}

	Draw();

	for(vector<Portal*>::iterator portal = leaf_portals_.begin(); portal != leaf_portals_.end(); portal++)
	{
		
		if(Renderer::render_state->current_cam->IsInFrustum((*portal)->polygon()))
		{
			//(*portal)->back_node()->DrawPortals();
			//(*portal)->front_node()->DrawPortals();
			(*portal)->Draw();
		}
	}


	for(vector<BSPNode*>::iterator node_in_pvs = pvs_.begin(); node_in_pvs != pvs_.end(); node_in_pvs++)
	{
		(*node_in_pvs)->Draw();
	}


	/*
	if(drawn_)
	{
		return;
	}

	Draw();
	BSPNode::num_drawn++;
	
	for(vector<Portal*>::iterator portal = portals_in_.begin(); portal != portals_in_.end(); portal++)
	{
		
		if(g_render_state.current_cam->IsInFrustum((*portal)->polygon()))
		{
			(*portal)->back_node()->DrawPortals();
			(*portal)->Draw();
		}
	}

	for(vector<Portal*>::iterator portal = portals_out_.begin(); portal != portals_out_.end(); portal++)
	{
		if(g_render_state.current_cam->IsInFrustum((*portal)->polygon()))
		{
			(*portal)->front_node()->DrawPortals();
			(*portal)->Draw();
		}
	}
	*/

}

vector<float> BSPNode::vertices_pos_list;
vector<float> BSPNode::vertices_uv_list;
vector<float> BSPNode::vertices_normal_list;


void BSPNode::Finish(vector<BSPNode*>& nodes_array){

	if(!has_polys_)
	{

		if(this == nodes_array[nodes_array.size() - 1])
		{
			vertices_pos_list.shrink_to_fit();
			vertices_uv_list.shrink_to_fit();
			vertices_normal_list.shrink_to_fit();

			cout << "num_pos: " << vertices_pos_list.size()/3 << endl;
			//int iii = 0;
		}

		return;
	}

	for(auto& my_poly : polygons_)
	{
		my_poly.normals.resize(my_poly.points.size());
		int my_ndx = &my_poly - &polygons_[0];
		for(unsigned int i = 0; i < my_poly.points.size(); i+=3)
		{
			my_poly.normals[i] = planes_[my_ndx].n[0];
			my_poly.normals[i + 1] = planes_[my_ndx].n[1];
			my_poly.normals[i + 2] = planes_[my_ndx].n[2];
			//cout << "normal1: " << my_poly.normals[i] << " " << my_poly.normals[i+1] << " " << my_poly.normals[i+2] << endl;
			for(auto& node : nodes_array)
			{
				if(node == this || !node->has_polys_)
				{
					continue;
				}
				for(auto& other_poly : node->polygons_)
				{
					if(other_poly.smoothing_group == my_poly.smoothing_group)
					for(unsigned int j = 0; j < other_poly.points.size(); j+=3)
					{
						if(	fabs(other_poly.points[j] - my_poly.points[i]) < epsilon 		&&
							fabs(other_poly.points[j+1] - my_poly.points[i+1]) < epsilon	&&
							fabs(other_poly.points[j+2] - my_poly.points[i+2]) < epsilon	)
						{
							int ndx = &other_poly - &node->polygons_[0];
							my_poly.normals[i] += node->planes_[ndx].n[0];
							my_poly.normals[i + 1] += node->planes_[ndx].n[1];
							my_poly.normals[i + 2] += node->planes_[ndx].n[2];

						}
					}
				}
			}
			for(unsigned int k = 0; k < polygons_.size(); k++)
			{
				if(k == my_ndx)
				{
					continue;
				}
				if(polygons_[k].smoothing_group == my_poly.smoothing_group)
				for(unsigned int j = 0; j < polygons_[k].points.size(); j+=3)
				{
					if(	fabs(polygons_[k].points[j] - my_poly.points[i]) < epsilon 		&&
						fabs(polygons_[k].points[j+1] - my_poly.points[i+1]) < epsilon	&&
						fabs(polygons_[k].points[j+2] - my_poly.points[i+2]) < epsilon	)
					{
						my_poly.normals[i] += planes_[k].n[0];
						my_poly.normals[i + 1] += planes_[k].n[1];
						my_poly.normals[i + 2] += planes_[k].n[2];
					}
				}

			}
			//normalize(my_poly.normals[i], my_poly.normals[i+1], my_poly.normals[i+2]);
			Normalize(&my_poly.normals[i]);
			//cout << "normal2: " << my_poly.normals[i] << " " << my_poly.normals[i+1] << " " << my_poly.normals[i+2] << endl;
		}
	}

	if(vertices_pos_list.empty())
	{
		vertices_pos_list.reserve(65000 * 3);
		vertices_uv_list.reserve(65000 * 2);
		vertices_normal_list.reserve(65000 * 3);
	}

	for(auto& my_poly : polygons_)
	{
		for(unsigned int i = 0; i < my_poly.points.size()/3; i++)
		{
			bool b = true;
			if(!vertices_pos_list.empty())
			for(unsigned int j = 0; j < vertices_pos_list.size()/3; j++)
			{
				if(	fabs(vertices_pos_list[j*3] 		- my_poly.points[i*3]) < epsilon 		&&
					fabs(vertices_pos_list[j*3+1] 		- my_poly.points[i*3+1]) < epsilon		&&
					fabs(vertices_pos_list[j*3+2] 		- my_poly.points[i*3+2]) < epsilon		&&

					fabs(vertices_uv_list[j*2] 			- my_poly.uvs[i*2]) < epsilon 			&&
					fabs(vertices_uv_list[j*2+1] 		- my_poly.uvs[i*2+1]) < epsilon			&&

					fabs(vertices_normal_list[j*3] 		- my_poly.normals[i*3]) < epsilon 		&&
					fabs(vertices_normal_list[j*3+1] 	- my_poly.normals[i*3+1]) < epsilon		&&
					fabs(vertices_normal_list[j*3+2] 	- my_poly.normals[i*3+2]) < epsilon		)
				{
					my_poly.vtx_indices.push_back(static_cast<unsigned short>(j));
					b = false;
					break;
				}
			}

			if(b)
			{
				vertices_pos_list.insert(vertices_pos_list.end(), &my_poly.points[i*3], &my_poly.points[i*3] + 3);
				vertices_uv_list.insert(vertices_uv_list.end(), &my_poly.uvs[i*2], &my_poly.uvs[i*2] + 2);
				vertices_normal_list.insert(vertices_normal_list.end(), &my_poly.normals[i*3], &my_poly.normals[i*3] + 3);

				my_poly.vtx_indices.push_back(static_cast<unsigned short>(vertices_pos_list.size()/3 - 1));
			}
		}

		vector<unsigned short> new_indices;
		for(unsigned int i = 1; i < my_poly.vtx_indices.size()-1; i++)
		{
			new_indices.push_back(my_poly.vtx_indices[0]);
			new_indices.push_back(my_poly.vtx_indices[i]);
			new_indices.push_back(my_poly.vtx_indices[i+1]);
		}
		my_poly.vtx_indices.swap(new_indices);

		cout << "num_ndx: " << my_poly.vtx_indices.size() << endl;

	}

	for(auto poly = polygons_.begin(); poly != polygons_.end(); poly++)
	{
		for(auto poly2 = polygons_.begin(); poly2 != polygons_.end(); poly2++)
		{
			if(poly == poly2)
			{
				continue;
			}

			if((*poly).tex == (*poly2).tex)
			{
				(*poly).vtx_indices.insert((*poly).vtx_indices.end(), (*poly2).vtx_indices.begin(), (*poly2).vtx_indices.end());
				(*poly2).vtx_indices.clear();
			}

		}
	}

}

void BSPNode::JoinVisibleFaces(){

	for(auto& poly : polygons_)
	{
		for(auto p_visible : pvs_)
		{
			for(auto& poly2 : p_visible->polygons_)
			{
				if(poly.tex == poly2.tex)
				{
					poly.vtx_indices.insert(poly.vtx_indices.end(), poly2.vtx_indices.begin(), poly2.vtx_indices.end());
					poly2.vtx_indices.clear();
				}
			}
		}
	}

}

void BSPNode::UpdateBbox(){
	if(!has_polys_)
	{
		front_node_->UpdateBbox();
		back_node_->UpdateBbox();

		bbox_min_[0] = min(back_node_->bbox_min_[0], front_node_->bbox_min_[0]);
		bbox_min_[1] = min(back_node_->bbox_min_[1], front_node_->bbox_min_[1]);
		bbox_min_[2] = min(back_node_->bbox_min_[2], front_node_->bbox_min_[2]);

		bbox_max_[0] = max(back_node_->bbox_max_[0], front_node_->bbox_max_[0]);
		bbox_max_[1] = max(back_node_->bbox_max_[0], front_node_->bbox_max_[1]);
		bbox_max_[2] = max(back_node_->bbox_max_[0], front_node_->bbox_max_[2]);
	}
}

void BSPNode::ReorderNodes(vector<BSPNode*>& nodes_array){

	nodes_array.push_back(this);
	id_ = nodes_array.size() - 1;
	cout << id_ << endl;
	if(front_node_)
		front_node_->ReorderNodes(nodes_array);
	if(back_node_)
		back_node_->ReorderNodes(nodes_array);

}

bool BSPNode::IsInPvs(BSPNode* node){

	auto p_node_it = std::find(pvs_.begin(), pvs_.end(), node);

	if(p_node_it != pvs_.end())
	{
		return true;
	} else
	{
		return false;
	}

}

void DeleteConnectedNodes(vector<BSPNode*>& nodes, int node_index, vector<Portal*>& portals) {

	if(!nodes[node_index])
	{
		return;
	}

	if(!nodes[node_index]->has_polys())
	{
		system("pause");
	}

	
	for(unsigned int j = 0; j < portals.size(); j++)
	{

		if(!portals[j])
		{
			continue;
		}

		BSPNode* next2delete = NULL;
		if(portals[j]->front_node() == nodes[node_index])
		{
			next2delete = portals[j]->back_node();
		} else
			if(portals[j]->back_node() == nodes[node_index])
			{
				next2delete = portals[j]->front_node();
			} else
			{
				continue;
			}

		portals[j] = NULL;
		for(unsigned int k = 0; k < nodes.size(); k++)
		{
			if(next2delete == nodes[k])
			{
				DeleteConnectedNodes(nodes, k, portals);
				break;
			}
		}

	}

	if(nodes[node_index])
	{
		BSPNode* parent = nodes[node_index]->parent_node();
		if(parent->front_node() == nodes[node_index])
		{
			parent->set_front_node(NULL);
		} else
		if(parent->back_node() == nodes[node_index])
		{
			parent->set_back_node(NULL);
		} else
		{
			cout << nodes[node_index] << endl;
			system("pause");
		}
	
		delete nodes[node_index];
		nodes[node_index] = NULL;
	}
	//nodes.erase(nodes.begin() + node_index);

}


Portal::Portal(Poly polygon, Plane plane) :	polygon_(polygon),
											plane_(plane),
											front_node_(NULL),
											back_node_(NULL){

	

}


Portal::Portal(Poly polygon, Plane plane, BSPNode* front, BSPNode* back) :	polygon_(polygon),
																			plane_(plane),
																			front_node_(front),
																			back_node_(back){

	

}
/*
Portal::Portal(const Portal& portal) {

	polygon_ = portal.polygon_;
	//polygon_.points.insert(polygon_.points.begin(), portal.polygon_.points.begin(), portal.polygon_.points.end());
	//copy(portal.polygon_.points.begin(), portal.polygon_.points.end(), polygon_.points.begin());
	plane_ = portal.plane_;

	front_node_ = portal.front_node_;
	back_node_ = portal.back_node_;

}*/

Portal::~Portal(){

}

void Portal::Draw() {
	glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	glDisable(GL_CULL_FACE);
	glUniform3f(Renderer::render_state->current_program->uniform(COL_UNIFORM), 1.0, 0.0, 0.0);
		
	glEnableVertexAttribArray(Renderer::render_state->current_program->attribute(POSITION_ATTRIBUTE));
	glVertexAttribPointer(Renderer::render_state->current_program->attribute(POSITION_ATTRIBUTE), 3, GL_FLOAT, GL_FALSE, 0, &polygon_.points[0]);
	
	glDrawArrays(GL_TRIANGLE_FAN, 0, polygon_.points.size() / 3);

	//glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
	glEnable(GL_CULL_FACE);
}
